Probabilistic state tracking with multi-head measurement model

ABSTRACT

A probabilistic system for tracking a state of a vehicle using unsynchronized cooperation of information includes a probabilistic multi-head measurement model relating incoming measurements with the state of the vehicle. The first head of the model relates measurements of the satellite signals subject to measurement noise with a belief on the state of the vehicle, and a second head relates an estimation of the state of the vehicle subject to estimation noise with the belief on the state of the vehicle. A probabilistic filter of the system updates recursively the belief on the state of the vehicle based on the multi-head measurement model accepting one or a combination of the measurements of the satellite signals subject to the measurement noise and the estimation of the state of the vehicle subject to the estimation noise.

TECHNICAL FIELD

This invention relates generally to positioning systems, such as theglobal positioning system (GPS) or the Quasi-Zenith Satellite System(QZSS), and more particularly to resolve a state of a vehicle usingunsynchronized cooperation of information.

BACKGROUND

A Global Navigation Satellite System (GNSS) is a system of satellitesthat can be used for determining the geographic location of a mobilereceiver with respect to the earth. Examples of GNSS include GPS,Galileo, Glonass, QZSS, and BeiDou. Various global navigation satellite(GNS) correction systems are known that are configured for receivingGNSS signal data from the GNSS satellites, for processing these GNSSdata, for calculating GNSS corrections from the GNSS data, and forproviding these corrections to a mobile receiver, with the purpose ofachieving quicker and more accurate calculation of the mobile receiver'sgeographic position.

Various position estimation methods are known wherein the positioncalculations are based on repeated measurement of the so-calledpseudo-range and carrier phase observables by Earth-based GNSSreceivers. The “pseudo-range” or “code” observable represents adifference between the transmit time of a GNSS satellite signal and thelocal receive time of this satellite signal, and hence includes thegeometric distance covered by the satellite's radio signal. Themeasurement of the alignment between the carrier wave of the receivedGNSS satellite signal and a copy of such a signal generated inside thereceiver provides another source of information for determining theapparent distance between the satellite and the receiver. Thecorresponding observable is called the “carrier phase”, which representsthe integrated value of the Doppler frequency due to the relative motionof the transmitting satellite and the receiver.

Any pseudo range observation comprises inevitable error contributions,among which are receiver and transmitter clock errors, as well asadditional delays caused by the non-zero refractivity of the atmosphere,instrumental delays, multipath effects, and detector noise. Any carrierphase observation additionally comprises an unknown integer number ofsignal cycles, that is, an integer number of wavelengths, that haveelapsed before a lock-in to this signal alignment has been obtained.This number is referred to as the “carrier phase ambiguity”. Usually,the observables are measured i.e. sampled by a receiver at discreteconsecutive times. The index for the time at which an observable ismeasured is referred to as an “epoch”. The known position determinationmethods commonly involve a dynamic numerical value estimation andcorrection scheme for the distances and error components, based onmeasurements for the observables sampled at consecutive epochs.

When GNSS signals are continuously tracked and no loss-of-lock occurs,the integer ambiguities resolved at the beginning of a tracking phasecan be kept for the entire GNSS positioning span. The GNSS satellitesignals, however, may be occasionally shaded (e.g., due to buildings in“urban canyon” environments), or momentarily blocked (e.g. when thereceiver passes under a bridge or through a tunnel). Generally, in suchcases, the integer ambiguity values are lost and must be re-determined.This process can take from a few seconds to several minutes. In fact,the presence of significant multipath errors or unmodeled systematicbiases in one or more measurements of either pseudo-range or carrierphase may make it difficult with present commercial positioning systemsto resolve the ambiguities. As the receiver separation (i.e., thedistance between a reference receiver and a mobile receiver whoseposition is being determined) increases, distance-dependent biases (e.g.orbit errors and ionospheric and tropospheric effects) grow, and, as aconsequence, reliable ambiguity resolution (or re-initialization)becomes an even greater challenge. Furthermore, loss-of-lock can alsooccur due to a discontinuity in a receiver's continuous phase lock on asignal, which is referred to as a cycle slip. For instance, cycle slipscan be caused by a power loss, a failure of the receiver software, or amalfunctioning satellite oscillator. In addition, cycle slip can becaused by changing ionospheric conditions.

GNSS enhancement refers to techniques used to improve the accuracy ofpositioning information provided by the Global Positioning System orother global navigation satellite systems in general, a network ofsatellites used for navigation. For example, some methods usedifferencing techniques based on differencing between satellites,differencing between receivers, differencing between epochs, andcombination thereof. Single and double differences between satellitesand the receivers reduce the error sources but do not eliminate them.

Consequently, there is a need to increase the accuracy of GNSSpositioning. To address this problem, a number of different methods usethe cooperation of multiple GNSS receivers to increase the accuracy ofGNSS positioning. However, to properly cooperate, the multiple GNSSreceivers need to be synchronized and their operation needs to beconstrained. For example, U.S. Pat. No. 9,476,990 describes cooperativeGNSS positioning estimation by multiple mechanically connected modules.However, such a restriction on the cooperative enhancement of accuracyof GNSS positioning is not always practical.

SUMMARY

Some embodiments are based on the realization that current methods oftracking of a state of a vehicle based on satellite signals receivedfrom a global navigational satellite system (GNSS) assume either anindividual or centralized estimation based on internal modules of thevehicle or a distributed estimation that performs the state estimationin a tightly controlled and/or synchronized manner. Examples of suchdistributed estimation include decentralized systems that determinedifferent aspects of the state tracking and estimate the state of thevehicle by reaching a consensus, imbalanced systems that track the stateof the system independently while one type of tracking is dominant overthe other, and distributed systems including multiple synchronized GNSSreceivers preferably located at a fixed distance from each other.

Some embodiments appreciate the advantages of cooperative state trackingwhen internal modules of a moving vehicle use some additionalinformation determined externally. However, some embodiments are basedon the recognition that such external information is not alwaysavailable. Hence, there is a need for a cooperative, but unsynchronizedstate tracking, when the tracking is performed by the internal modulesof the vehicle but can seamlessly integrate the external informationwhen such information is available.

Some embodiments are based on the realization that various probabilisticfilters that recursively track the state of a vehicle include two parts.The first part estimates the samples of the state of the vehicle and thesecond part updates the probabilistic distribution of the state based onthese estimated samples. At first glance, these two parts are integratedinto each other because one part is not executed without the other.However, some embodiments are based on the realization that these twoparts are related by causal dependency, not a dependency of time.Specifically, the probabilistic update is dependent on the presence ofthe state sample estimation, in the sense that when the new state sampleis estimated, its arrival should trigger the update. However, when andhow the state sample is estimated is independent of the update of theprobabilistic distribution.

Some embodiments exploit this understanding to disambiguate theprobabilistic state estimation and update parts. This disambiguationallows reusing the probabilistic update regardless of the time and themanner the state sample is estimated. Hence, the state estimate can bedetermined by internal modules of the vehicle based on GNSS measurementsor by modules external to the vehicle when such modules are available.Regardless of the principles of state estimation, the probabilisticupdate operates in the same manner and its quality depends not on thefrequency of arrival of new information, but the quality of itsestimation. In such a manner, the internal and external state estimatorscan operate in an unsynchronized manner.

Based on these understandings, some embodiments disclose a multi-headmeasurement model allowing a probabilistic filter to use differentsources to update the belief on the state of the vehicle. The multi-headmeasurement model includes multiple paths producing different types ofoutputs but having the same probabilistic structure. For example, insome implementations, the multi-head measurement model includes twoheads. The first head relates measurements of the satellite signals witha belief on a state of the vehicle subject to measurement noise, and thesecond head relates an estimation of the state of the vehicle with thebelief on the state of the vehicle subject to estimation noise. Hence,the measurement model includes different information and different typesof noise but having a similar structure acceptable by the probabilisticfilter allowing use of outputs of different heads either individually orjointly.

In such a manner, in some embodiments, the probabilistic filterpropagates recursively, at each update step, the parameters of theprobabilistic distribution of the state of the vehicle according to amotion model of state transitions of the vehicle subject to processnoise and updates the parameters of the probabilistic distribution uponreceiving outputs of one or a combination of the first head ofmulti-head measurement model and the second head of multi-headmeasurement model.

For example, the first head allows updating the belief of the state ofthe vehicle using GNSS measurements having specific uncertainties,biases, and ambiguities affecting accuracy and/or quality of stateestimation. The outputs of the first head are usually available with thefrequency of updates of the probabilistic filter. In contrast, the firsthead allows updating the belief of the state of the vehicle usingexternal information that may be received only occasionally but may havegreater accuracy of state estimation. For example, the second head canprocess state estimations received from an external measurement module.Hence, the second head may receive information less frequently, butregardless of the frequency of execution of the first and/or the secondheads of the multi-head measurement model, outputs of the first and thesecond heads are seamlessly integrated together without breakingprobabilistic guarantees of the probabilistic filter.

Some embodiments are based on understanding that the externalmeasurement module can be passive, active, or both. In the passive mode,the external measurement module receives the estimation of the state ofthe vehicle determined independently from GNSS measurements performed bythe internal measurement module. For example, the vehicle can pass neara roadside unit (RSU) configured to estimate the state of the vehicleusing various telemetric techniques. In the active mode, the vehicle cantransmit to a remote server, which also can be an RSU, currentinformation of the state of the vehicle and receive in response theexternal measurements made based on the correction of the internalmeasurements. Here, current information includes a mean of the stateestimate of the vehicle, a covariance of the estimate of the state ofthe vehicle, a set of code and carrier phase measurements used by theinfernal measurement module to update its internal state.

To that end, it is an object of some embodiments to provide acooperative, but the unsynchronized estimation of the state of thevehicle suitable for being used by the probabilistic state estimation.It is another object of some embodiments to provide such cooperative,unsynchronized estimation when the external active module executes at anupdate rate that is possibly different from the internal probabilisticfilter execution.

Some embodiments are based on a recognition that the GNSS positioningproblem concerns the estimation of a receiver's states from a set ofcode and carrier-phase measurements received from one or severalconstellations of satellites. The involved measurement equations aretime-varying, nonlinear in the position of the receiver, and incorporatevarious biases and integer ambiguities. In the carrier-phasemeasurements, there is an integer bias known as the ambiguity, unique toeach carrier-phase measurement from each satellite. When consideringthese biases in time, they follow an integer jump process, remainingconstant before sporadically and independently of each other jumping tonew integer values, commonly referred to as “cycle slip”. Hence, theGNSS positioning problem can be seen as a mixed-integer GNSS positioningproblem solving a mixed-integer problem of ever-increasing size (withnew integer biases included in each time-step), and carefulconsiderations need to be made in how to best relax this estimationproblem to make the resulting algorithm implementable.

Some embodiments are based on the realization that in the setting ofprobabilistic filters, such that the uncertainty of the integerambiguity resulting from e.g. a cycle slip, is reflected into a secondmoment, i.e., a covariance, of the probabilistic distribution of thestate of the vehicle. To that end, to benefit from the active mode ofthe external measurement module, some embodiments transmit theparameters of the probabilistic distribution to allow the remote serverto correct this information. The remote server can collect similarinformation from other vehicles in the proximity, fuse the receivedinformation to correct the parameters of the probabilistic distributionof the state of the vehicle, and transmit the updated probabilisticparameters back to the vehicle. Upon receiving the updated probabilisticparameters, the external measurement module estimates the state of thevehicle at a current instant of time consistent with the updatedparameters and triggers the execution of the probabilistic filter. Forexample, the external measurement module can propagate the mean of theprobabilistic distribution in time based on a motion model of thevehicle, sample the updated probabilistic distribution defined by thepropagated mean and received variance, and use the probabilistic filterwith the sampled state estimations to update the probabilisticdistribution of the state of the vehicle.

In some embodiments, the state of a vehicle includes a position of avehicle, a velocity of a vehicle, integer ambiguities associated withcarrier phase measurements of a vehicle from specific satellites, andresidual bias states modeling the atmospheric, e.g., ionospheric andtropospheric, delays. For example, some methods use differencingtechniques based on differencing between satellites, differencingbetween receivers, differencing between epochs, and combination thereof.Single and double differences between satellites and the receiversreduce the error sources but do not eliminate them, thereby reducing theaccuracy of state estimation.

Some embodiments are based on the realization that ignoring state biasessuch as ionospheric errors can lead to slight inaccuracies of stateestimation. This is because biases are usually removed by single ordouble differencing of GNSS measurements. This solution works well whenthe desired accuracy for position estimation of a vehicle is in theorder of meters but can be a problem when the desired accuracy is in theorder of centimeters. To that end, some embodiments include state biasesin the state of the vehicle and determine them as part of the statetracking provided by the probabilistic filter.

Some embodiments are based on a recognition that a multi-headmeasurement model enabling unsynchronized but cooperative communicationwith external servers can address the problem of the accuracy of stateestimation when the external state estimation determined at a remoteserver is sufficiently accurate. However, there is still a need for thatserver to determine the state with the desired accuracy.

Some embodiments recognize that for an individual vehicle it can beimpractical to determine accurately atmospheric time delays, as for anindividual vehicle the probabilistic filter needs to resolve bothuncertainties in the integer ambiguity and the atmospheric delays.However, atmospheric delays are similar for vehicles that aresufficiently close, e.g., 2-10 km, to each other. Hence, for an externalactive module gathering similar information from multiple vehicles,there is significant overlap between states of different vehicles, suchthat the external measurement module can update the state of anindividual vehicle in manners that the probabilistic filter of theindividual vehicle cannot. For example, ionospheric state biases can beshared among vehicles in proximity to each other.

Additionally or alternatively, some embodiments are based on therealization that the inaccuracy of state estimation determined by theprobabilistic filters can be caused by the ambiguities and residualdelays, but also by measurement noise. Some embodiments are based on therealization that when the states of multiple vehicles are determinedcooperatively, the states of the vehicle may or may not overlap, theambiguities or delays may or may not overlap, but the measurement noiseof different vehicles have a correlation that can be explored bycooperative state estimation. For example, errors in the measurements ofdifferent pairs of vehicles can be related to each other.

To that end, some embodiments perform probabilistic tracking of multiplevehicles based on an augmented probabilistic filter that combinesinformation from different vehicles into the augmented domain. Forexample, the augmented probabilistic filter fuses the states of themultiple vehicles into an augmented state, fuses the measurements ofsatellite signals of the multiple vehicles into an augmented measurementof the augmented state subject to augmented measurement noise defined bya non-diagonal covariance matrix. This non-diagonal covariance matrixhas non-zero off-diagonal elements capturing the correlation between themeasurement noise of different vehicles. This correlation is theadditional information that the augmented probabilistic filter canexplore for jointly tracking states of multiple vehicles.

In some embodiments, the probabilistic filter is a mixed-integerleast-squares Kalman filter, where the nonlinear measurement equation islinearized around its current estimate, resulting in a mixed-integerextended Kalman filter. Other embodiments recognize that thelinearization in the EKF may be inaccurate. Hence, in some embodiments,the probabilistic filter is a mixed-integer linear regression Kalmanfilter. Such a filter more accurately represents the first and secondmoment of the probabilistic distribution but is significantly moreexpensive computationally.

In one embodiment, it is recognized that parts of the state of avehicle, integer ambiguity, is linear in the model but the position isnonlinear. Hence, parts of the first and second moment can be determinedanalytically, whereas others are estimated with a linear-regressionKalman filter.

The GNSS positioning problem can be seen as the mixed-integer GNSSpositioning problem solving a mixed-integer problem of ever-increasingsize (with new integer biases included in each time-step), and carefulconsiderations need to be made in how to best relax this estimationproblem to make the resulting algorithm implementable. Especially forthe external estimation of the state when combining information frommultiple vehicles, the resulting estimation problem will be veryhigh-dimensional and therefore computationally expensive.

In some embodiments, the relaxation is solved by determining two relatedbut separate probabilistic distributions, wherein one distribution onlyconsiders real-valued parameters, i.e., ignores that the ambiguities arein fact integers, and the second distribution is determined based on aninteger ambiguity determined from the first distribution by solving anoptimization problem.

Accordingly, one embodiment discloses a probabilistic system fortracking a state of a vehicle using unsynchronized cooperation ofinformation received from satellite signals transmitted by a globalnavigation satellite system (GNSS) and information transmitted overradio frequency (RF) channel. The probabilistic system includes a memoryconfigured to store a probabilistic multi-head measurement modelrelating incoming measurements with the state of the vehicle, whereinthe probabilistic multi-head measurement model includes a first headrelating measurements of the satellite signals subject to measurementnoise with a belief on the state of the vehicle, and a second headrelating an estimation of the state of the vehicle subject to estimationnoise with the belief on the state of the vehicle; and at least oneprocessor configured to process executable instructions of modules ofthe probabilistic system.

The modules of the probabilistic system include a GNSS measurementmodule operatively connected to a GNSS receiver and configured todetermine the measurements of the satellite signals and the measurementnoise; an RF measurement module operatively connected to an RF receiverof the vehicle and configured to receive data indicative of theestimation of the state of the vehicle and the estimation noise; and aprobabilistic filter configured to update recursively parameters of aprobabilistic distribution of the state of the vehicle to produce thebelief on the state of the vehicle based on the multi-head measurementmodel accepting one or a combination of the measurements of thesatellite signals subject to the measurement noise and the estimation ofthe state of the vehicle subject to the estimation noise.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1A shows a schematic of a global navigation satellite system (GNSS)according to some embodiments.

FIG. 1B shows an example when there are two additional vehicles 140 and150 equipped with GNSS receivers.

FIG. 1C shows the various variables that are used alone or incombination in the modeling of the motion and/or measurement modelaccording to some embodiments.

FIG. 1D shows an illustration of a scenario where multipath disturbs thesignals for the receiver.

FIG. 1E shows a conceptual diagram of a probabilistic filter used bysome embodiments.

FIG. 1F shows an illustration of asynchronous receiving of differentsignals according to some embodiments.

FIG. 1G shows an illustration of the structure of the multi-headmeasurement model according to some embodiments.

FIG. 1H shows a flowchart of an example of how to update the first andsecond moment of state estimation at a time instance according to oneembodiment.

FIG. 1I shows another flowchart of a method for updating the first andsecond moment of state estimation according to another embodiment.

FIG. 1J shows a reversed situation, where only the first and secondmoment of state estimation have been received at a particular timeinstance according to another embodiment.

FIG. 1K shows a flowchart where the recognition that the update usingthe first and second head can be done jointly, i.e., simultaneously.

FIG. 2A shows a flowchart of a method for tracking a state of a vehicleusing unsynchronized cooperation of information according to someembodiments.

FIG. 2B shows a probabilistic system for tracking a state of a vehicleusing unsynchronized cooperation of information according to someembodiments.

FIG. 2C shows a flowchart of an example of a method for determining 240a the estimates according to some embodiments.

FIG. 2D shows an example of a method for determining belief on the stateof the vehicle according to some embodiments.

FIG. 3A shows a flowchart of a method of an exemplar implementation forstate estimation of a vehicle according to some embodiments.

FIG. 3B shows a flowchart of a method for determining carrier phaseambiguities according to some embodiments.

FIG. 3C shows a flowchart of an exemplar implementation of the methodthat samples the float values consistent with the motion model and itsprocess noise according to one embodiment.

FIG. 3D shows a schematic illustrating some principles employed by someembodiments.

FIG. 4 shows a flowchart of a method for executing a probabilisticfilter to update parameters of probabilistic distribution of a state ofa vehicle according to one embodiment.

FIG. 5A shows a traffic scenario illustrating a scenario according tosome embodiments.

FIG. 5B shows the synchronization message exchange in IEEE PrecisionTime Protocol (PTP) based synchronization.

FIG. 5C shows a probabilistic system for tracking a state of a vehicleusing unsynchronized cooperation of information according to someembodiments.

FIG. 5D shows a traffic scenario according another embodiment.

FIG. 6A shows a flowchart of a recursive method for determining a firstand second moment of the state of a vehicle using multiple transmissionsfrom multiple vehicles according to some embodiments.

FIG. 6B shows a flowchart of a recursive method for updating theparameters of augmented state at the remote server according to someembodiments.

FIG. 6C shows a flowchart of a method for fusing multiple states of thevehicle with the augmented state according to some embodiments.

FIG. 6D shows an example of the overlapping of state space according tosome embodiments.

FIG. 6E shows a flowchart of a method for fusing the augmented statewith the state of vehicles according to some embodiments.

FIG. 6F shows a flowchart of a method for determining the crosscovariance according to one embodiment.

FIG. 6G shows an example of cross covariance in the measurement noise ofthe full measurement model including measurements from multipletransmissions according to some embodiments.

FIG. 6H shows a flowchart of a method for determining the measurementnoise matrix such that it results in the structure in FIG. 6G.

FIG. 7 shows a probabilistic system for tracking a state of multiplevehicles using unsynchronized cooperation of information received fromvehicles transmitted over an RF channel according to some embodiments.

FIG. 8 shows an example of a vehicle-to-vehicle (V2V) communication andplanning based on state estimation according to one embodiment.

FIG. 9 is a schematic of a multi-vehicle platoon shaping for accidentavoidance scenario according to one embodiment.

FIG. 10 shows a block diagram of a system 1000 for direct and indirectcontrol of mixed-autonomy vehicles in accordance with some embodiments.

FIG. 11A shows a schematic of a vehicle 1101 controlled directly orindirectly according to some embodiments.

FIG. 11B shows a schematic of interaction between controller 1102receiving controlled commands from the system 1000 and the controller1100 of the vehicle 1101 according to some embodiments.

DETAILED DESCRIPTION

FIG. 1A shows a schematic of a global navigation satellite system (GNSS)according to some embodiments. For instance, the Nth satellite 102transmits 120 and 121 code and carrier phase measurements to a set ofreceivers 130 and 131. For example, receiver 130 is positioned toreceive signals 110, 120, from N satellites 101, 103, 104, and 102.Similarly, receiver 131 is positioned to receive signals 121 and 111from the N satellites 101, 103, 104, and 102.

In various embodiments, the GNSS receivers 130 and 131 can be ofdifferent types. For example, in the exemplar embodiment of FIG. 1A,receiver 131 is a base receiver, whose position is known. For instance,receiver 131 can be a receiver mounted on the ground. In contrast,receiver 130 is a mobile receiver configured to move. For instance, thereceiver 130 can be mounted in a cell phone, a car, or a tablet. In someimplementations, the second receiver 131 is optional and can be used toremove, or at least decrease, uncertainties and errors due to varioussources, such as atmospheric effects and errors in the internal clocksof the receivers and satellites. In some embodiments, there are multipleGNSS receivers receiving code and carrier phase signals.

FIG. 1B shows an example when there are two additional vehicles 140 and150 equipped with GNSS receivers. The satellites can transmit code andcarrier phase signals to several receivers, such that the receiversshare the same source of the code and carrier phase signals. Forexample, satellite 104 can transmit signals to both receiver 140 andreceiver 150, and satellite 101 can transmit signals to both receivers130, 140, and 150. Additionally or alternatively, the receivers of GNSSsignals can exchange information with each other on radio waves 161.

It is an objective of some embodiments to disclose a system and methodfor improving the satellite-based tracking of a state of the vehicle,wherein the vehicle is equipped with a GNSS receiver. It is anotherobjective to provide such a system and method using unsynchronizedcooperation of information received from satellite signals. It is yetanother objective of some embodiments to provide such a system andmethod that is probabilistic, i.e., it accounts for probabilisticdisturbances and error sources. It is an objective of other embodimentsto track the state of a vehicle using different information fromdifferent sources and not only rely on satellite signals. For example,in some embodiments, the state of the vehicle is tracked using GNSSsignals received from satellites using a GNSS receiver and a first andsecond moment of the probabilistic distribution of the state of thevehicle received from a remote server using a radio frequency (RF)receiver.

Additionally, or alternately, it is an objective of some embodiments toprovide filters that are probabilistic. The filters are probabilisticfilters such as state estimation filters, providing state estimatesbased on a motion model and a measurement model.

An example of a state estimator is a Kalman filter, which uses a seriesof measurements observed over time, containing statistical noise andother inaccuracies, and produces estimates of unknown variables thattend to be more accurate than those based on a single measurement alone,by estimating a joint probability distribution over the variables foreach time frame. The Kalman filter keeps track of the estimated state ofthe system and the uncertainty of the estimate. The estimate is updatedusing the motion model of state transitions and the measurements. Someembodiments use a Kalman filter-based system with a motion model subjectto process noise of a GNSS receiver and a measurement model of satellitesignals subject to measurement noise. In some embodiments, themeasurement model is probabilistic and multi-head, i.e., includesmultiple paths producing different types of outputs. In someimplementations, the multi-head measurement model includes two heads.The first head relates measurements of the satellite signals with abelief on a state of the vehicle subject to measurement noise, andwherein the second head relates an estimation of the state of thevehicle with the belief on the state of the vehicle subject toestimation noise. Hence, the measurement model includes differentinformation and different types of noise but having a similar structureacceptable by the probabilistic filter allowing the use of outputs ofdifferent heads either individually or jointly.

In such a manner, in some embodiments, the probabilistic filterpropagates recursively, at each update step, the parameters of theprobabilistic distribution of the state of the vehicle according to amotion model of state transitions of the vehicle subject to processnoise and updates the parameters of the probabilistic distribution uponreceiving outputs of one or a combination of the first head ofmulti-head measurement model and the second head of multi-headmeasurement model.

In some embodiments, the model of the motion of a receiver is ageneral-purpose kinematic constant-acceleration model with the statevector χ_(k)=[p_(r,k)v_(r,k)a_(r,k)]^(T), where the three components arethe position, velocity, and acceleration of the receiver. In some otherembodiments, the time evolution ambiguity of propagation of thesatellite signals is modeled as n_(k+1)=n_(k)=w_(n,k), w_(n,k)˜

(0,Q_(n)), where n_(k+1)) is the ambiguity and w_(n,k) is the Gaussianprocess noise with covariance Q_(n).

In some embodiments, ambiguity is included in the state of the vehicle.Other embodiments also include bias states capturing residual errors inthe atmospheric delays, e.g., ionospheric delays. For receiverssufficiently close to each other, the ionospheric delays are the same,or very similar, for different vehicles. Some embodiments utilize thisto relationships to resolve these delays and/or ambiguities.

Some embodiments capture the carrier and code signals in the measurementmodel y_(k)=h_(k)+λn _(k)+e_(k), where e_(k) is the measurement noise, his a nonlinear part of the measurement equation dependent on theposition of the receiver, n is the integer ambiguity, λ is a wavelengthof the carrier signal, and y is a single or double difference between acombination of satellites K.

In some embodiments, the probabilistic filter uses the carrier phasesingle difference (SD) and/or double difference (DD) for estimating astate of the receiver indicating a position of the receiver. When acarrier signal transmitted from one satellite is received by tworeceivers the difference between the first carrier phase and the secondcarrier phase is referred to as the single difference (SD) in thecarrier phase. Alternatively, the SD can be defined as the differencebetween signals from two different satellites reaching a receiver. Forexample, the difference can come from a first and a second satellitewhen the first satellite is called the base satellite. For example, thedifference between signal 110 from satellite 101 and signal 120 fromsatellite 102 is one SD signal, where satellite 101 is the basesatellite. Using pairs of receivers, 131 and 130 in FIG. 1A, thedifference between SDs in the carrier phase obtained from the radiosignals from the two satellites is called the double-difference (DD) inthe carrier phase. When the carrier phase difference is converted intothe number of wavelength, for example, λ=19 cm for L1 GPS (and/or GNSS)signal, it is separated by fractional and integer parts. The fractionalpart can be measured by the positioning apparatus, whereas thepositioning device is not able to measure the integer part directly.Thus, the integer part is referred to as integer bias or integerambiguity.

In general, a GNSS can use multiple constellations at the same time todetermine the receiver state. For example, GPS, Galileo, Glonass, andQZSS can be used concurrently. Satellite systems typically transmitinformation at up to three different frequency bands, and for eachfrequency band, each satellite transmits a code measurement and acarrier-phase measurement. These measurements can be combined as eithersingle differenced or double differenced, wherein a single differenceincludes taking the difference between a reference satellite and othersatellites, and wherein double differencing includes differencing alsobetween the receiver of interest and a base receiver with a known staticlocation.

FIG. 1C shows the various variables that are used alone or incombination in the modeling of the motion and/or measurement modelaccording to some embodiments. Some embodiments model the carrier andcode signals for each frequency with the measurement modelP _(k) ^(j)=ρ_(k) ^(j) +c(δt _(r,k)−δ_(k) ^(j))+I _(k) ^(j) +T _(k)^(j)+ε_(k) ^(j)  (1)Φ_(k) ^(j)=ρ_(k) ^(j) +c(δt _(r,k) −δt _(k) ^(j))=I _(k) ^(j) +T _(k)^(j) +λn ^(j)+η_(k) ^(j)  (2)where P^(j) is the code measurement ρ^(j) is the distance between thereceiver and the j th satellite, c is the speed of light,

is the receiver clock bias, δt^(j) is the satellite clock bias, I^(j) isthe ionospheric delay, T^(j) is the tropospheric delay, ε^(j) is theprobabilistic code observation noise, Φ^(j) is the carrier-phaseobservation, λ is the carrier wavelength, n^(j) is the integerambiguity, and η^(j) is the probabilistic carrier observation noise.

In one embodiment, the original measurement model is transformed byutilizing a base receiver b mounted at a known location broadcasting tothe original receiver r, most of the sources of error can be removed.For instance, one embodiment forms the difference between the tworeceivers 130 and 131 in FIG. 1A as ΔP_(br,k) ^(j)=P_(b,k) ^(j)−P_(r,k)^(j) and ΔΦ_(br,k) ^(j)=Φ_(b,k) ^(j)−Φ_(r,k) ^(j), from which the errordue to the satellite clock bias can be eliminated. Another embodimentforms a double difference between two satellites j and l. Doing in sucha manner, clock error terms due to the receiver can be removed.Furthermore, for short distances between the two receivers (e.g., 30km), the ionospheric errors can be ignored, at least when centimeterprecision is not needed, leading to ∇ΔP_(br,k) ^(jl)≈∇Δρ_(br,k)^(jl)+∇Δε_(br,k) ^(jl), ∇ΔΦ_(br,k) ^(jl)≈∇Δρ_(br,k) ^(jl)+λ∇Δn_(br)^(jl)+∇Δη_(br,k) ^(jl). Alternatively, one embodiment forms thedifference between two satellites 101 and 102, leading to SDmeasurements.

Additionally or alternatively, some embodiments are based on therealization that ignoring state biases such as ionospheric errors canlead to slight inaccuracies of state estimation. This is because biasesare usually removed by single or double differencing of GNSSmeasurements. This solution works well when the desired accuracy forposition estimation of a vehicle is in the order of meters but can be aproblem when the desired accuracy is in the order of centimeters. Tothat end, some embodiments include state biases in the state of thevehicle and determine them as part of the state tracking provided by theprobabilistic filter.

In certain scenarios, e.g., deep urban canyons, there are multipledistortions of satellite signals such that the information contained inthe code and carrier phase signals makes it difficult to performhigh-precision state estimation. For instance, FIG. 1D shows anillustration of a scenario where a multipath disturbs the signals forthe receiver 101 d. The receiver 101 d receives various signals 109 dand 119 d from satellites 110 d and 120 d. There are other satellites130 d and 140 d that transmit signals 128 d, 129 d, 138 d, 139 d, butdue to an obstruction 170 d, for instance, a building in urban areas,these signals are not directly transmitted to the receiver.

Previously, the signal 138 d sent from the satellite 140 d was notavailable, but suddenly the satellite signal 139 d reaches the receiverafter a multipath 102 d. Such a scenario can severely detriment theperformance of the probabilistic filter in tracking the state of thevehicle, because the estimator locks on to the wrong ambiguity estimate,thus causing a large estimation error. However, there might be otherreceivers nearby that don't have such an issue, e.g., one receiver mayhave multipath and few visible satellites due to being shadowed by alarge building, whereas a vehicle several blocks away has goodestimation performance. In some embodiments, this is utilized byupdating the parameters of the probabilistic distribution in theprobabilistic filter by using measurements from a GNSS measurementmodule as well as remote estimates of the state of the vehicle, whereinthe remote estimates are determined by using data from multiplevehicles.

An example of a probabilistic filter is the Kalman filter. FIG. 1E showsa schematic of the Kalman filter (KF) used by some embodiments for stateestimation. The KF is a tool for state estimation in linear state-spacemodels, and it is the optimal estimator when the noise sources are knownand Gaussian, in which case also the state estimate is Gaussiandistributed. The KF estimates the mean and variance of the Gaussiandistribution, because the mean and the variance are the two requiredquantities, sufficient statistics, to describe the Gaussiandistribution.

The KF starts with an initial knowledge 110 e of the state, to determinea mean of the state and its variance 111 e. The KF then predicts 120 ethe state and the variance to the next time step, using a model of thesystem, such as the motion model of the vehicle, to obtain an updatedmean and variance 121 e of the state. The KF then uses a measurement 130e in an update step 140 e using the measurement model of the system, todetermine an updated mean and variance 141 e of the state. An output 150e is then obtained, and the procedure is repeated for the next time step160 e.

Some embodiments employ a probabilistic filter including variousvariants of KFs, e.g., extended KFs (EKFs), linear-regression KFs(LRKFs), such as the unscented KF (UKF). Even though there are multiplevariants of the KF, they conceptually function as exemplified by FIG.1E. Notably, the KF updates the first and second moment, i.e., the meanand covariance, of the probabilistic distribution of interest, using ameasurement 130 e described by a probabilistic measurement model. Insome embodiments, the probabilistic measurement model is a multi-headmeasurement model structured to satisfy the principles of measurementupdates in the KF.

Some embodiments are based on the recognition that the outputs ofdifferent heads of the multi-head measurement model of some embodimentsneed not be synchronous, allowing the signals to be received atdifferent time steps. Other embodiments are based on the understandingthat to execute a KF as is, the measurement model needs to be structuredsuch that the KF can accept the measurement model.

FIG. 1F shows an illustration of asynchronous receiving of differentsignals according to some embodiments. As time progresses, the GNSSsignals are received at certain time steps, and the first and secondmoments are received at some other time steps. For example, the GNSSsignal and first and second moment are both received at time 110 f.However, the second GNSS signal is received at time 120 f, whereas thesecond receiving the first and second moment are received at time 130 f.I.e., sometimes the signals are received at the same time, sometimesthey are not. In order to process such signals possibly received atdifferent time steps, the measurement model is structured to be on aform such that different signals can be used in the KF independentlyfrom each other.

To that end, in some embodiments, the measurement model is probabilisticand multi-head, i.e., includes several parts, wherein the first headrelates measurements of the satellite signals with a belief on a stateof the vehicle subject to measurement noise, and wherein the second headrelates an estimation of the state of the vehicle with the belief on thestate of the vehicle subject to estimation noise. Hence, the measurementmodel includes different information and different types of noise.

In one embodiment the first head of the measurement model subject tomeasurement noise is used to associate GNSS measurements with a beliefof the state of the vehicle, whereas the second head of the measurementmodel subject to measurement noise is used to associate an estimate ofthe state of the vehicle with the belief of the state of the vehiclereceived by other means.

Some embodiments are based on the principles of processing themeasurements using the measurement model to correct and update 140 e thefirst and second moment using the measurement 130 e and the measurementmodel. To use a KF, the measurement model and measurement noise must bewritten in a probabilistic form such that they can be processed in abalanced way, such that the measurements do not scale the first andsecond moments incorrectly.

Other embodiments are based on a recognition that the probabilisticstructure needs to be reconciled among the first and second heads of themeasurement model. Specifically, regardless of whether the update step140 e of the probabilistic distribution of the state of the vehicle isdone using the GNSS signals or the received estimation of the state ofthe vehicle and the estimation noise, e.g., represented as first andsecond moment, the outputs have the same structure.

FIG. 1G shows an illustration of the structure of the multi-headmeasurement model 100 g according to some embodiments. The multi-headmeasurement model accepts inputs, relates GNSS measurements to thebelief of the state of the vehicle using the first head 110 g of themeasurement model and alternative estimates of the state of the vehicleto the belief of the state of the vehicle using the second head 120 g.For example, the multi-head measurement model accepts GNSS measurements130 g and measurement noise 140 g. Then, the multi-head measurementmodel transforms the measurement noise 140 g to a second momentrepresenting 150 g the measurement noise. For example, the measurementnoise is represented 150 g using a Gaussian distribution having zeromean and covariance R_(k). Using the representation 150 g of measurementnoise, the first head 110 g relates 111 g the measurement 130 g with thebelief 160 g of the state of the vehicle. The output 112 g from thefirst head of the measurement model is on a form that can be accepted bythe KF. For example, in one embodiment the output 112 g is the relation111 g described as an error between the measurement and the belief ofthe state inserted into the measurement model and the measurement noise.In another embodiment, the output 112 g is the relation 111 g decomposedinto the measurement, the belief of the state inserted into themeasurement model, and the measurement noise covariance.

In one embodiment, the inputs to the second head of the measurementmodel are the estimate 170 g of the state of the vehicle and estimationnoise 180 g. The multi-head measurement model transforms 190 g theestimation noise to the same format as the transforming 150 g. Forexample, if the measurement noise is described by a Gaussiandistribution, the estimation noise needs to be described by a Gaussiandistribution. Using the estimation of the state and the representation190 g of estimation noise, the second head 120 g relates 121 g theestimate to the belief 160 g of the state of the vehicle. In someembodiments the relating is an affine relation χ_(k)={circumflex over(χ)}_(k)+v_(k), wherein v_(k) is the estimation noise represented on aparticular form and {circumflex over (χ)}_(k) is the belief of the stateof the vehicle, e.g., represented by the first moment determined by theprobabilistic filter or a sample of the distribution determined by theprobabilistic filter.

Some embodiments are based on the understanding that the representationof the outputs 112 g and 122 g on the same probabilistic form enablesthe updating in the KF done in the same principles irrespective ofwhether the updating information is 112 g or 122 g, i.e., whether theinformation arises from the first head or the second head.

Other embodiments are based on the recognition that the update using thefirst and head and the update using the second head can be donesequentially or jointly. FIG. 1H shows a flowchart of an example of howto update the first and second moment at a time instance according toone embodiment. Using the output 112 g from the multi-head measurementmodel, the method updates 110 h the first and second moment using thefirst head of the measurement model. The updated moments 111 h are thenupdated 120 h again using the output 122 g from the second head,resulting in the updated moments 121 h.

Other embodiments are based on the understanding that the GNSSmeasurements and received estimation of the state of the vehicle andestimation noise may not be received at the same time instance, asexemplified in FIG. 1F. FIG. 1I shows another flowchart of a method forupdating the first and second moment, e.g., using a KF. Using the output112 g from the first head of the measurement model, the method updates110 h the moment 111 h.

FIG. 1J shows a reversed situation, where only the first and secondmoments have been received at a particular time instance. Hence, oneembodiment updates 110 h the moments 111 j using the output 122 g fromthe second head of the measurement model.

Other embodiments are based on the recognition that the update using thefirst and second head can be done jointly, i.e., simultaneously. FIG. 1Kshows a flowchart of an example of such a procedure. Using the output112 g from the first head and the output 122 g from the second head, themethod updates 110 k the moments using the first and second head,resulting in updated moments 111 k. In one embodiment, the jointupdating is done by stacking the two heads of measurement models,resulting in an augmented output of the measurement model. I.e., theGNSS measurement and the estimation of the state of the vehicle andassociated relations are merged, resulting in a joint and large update.

FIG. 2A shows a flowchart of a method for tracking a state of a vehicleusing unsynchronized cooperation of information according to someembodiments. In the embodiments, the information is received fromsatellite signals transmitted by a global navigation satellite system(GNSS) and also transmitted over a radio frequency (RF) channel First,the method receives 210 a from a GNSS receiver 209 a signals transmittedfrom a set of satellites 101, 102, 103, 104 according to someembodiments. The signals include a code signal and a carrier phasesignal, wherein each carrier signal includes a carrier phase ambiguityas an unknown integer number of wavelengths of the carrier signaltraveled between the satellite 101, 102, 103, or 104 and the vehicle130. Using the GNSS signals 215 a, the method determines 220 ameasurements of the satellite signals and the corresponding measurementnoise 225 a. The method also receives 230 a from an RF receiver 229 a ofthe vehicle via RF signals 235 a the estimation of the state of thevehicle and the estimation noise determined by the external server. Invarious embodiments, the estimation and the estimation noise 235 a aredetermined probabilistically as the first moment of the state of thevehicle and a second moment 235 a of the state of the vehicle.Additionally or alternatively, the estimation and the estimation noise235 a can be transformed into the first moment of the state of thevehicle and a second moment of the state of the vehicle using variousprobabilistic techniques.

Using the first and second moment, the method determines 240 a anestimate of the state of the vehicle and an associated estimation noiseof the estimation of the state of the vehicle. Using the estimates 245a, measurements 225 a, and a multi-head measurement model 249 a, themethod relates 250 a the GNSS measurements 225 a with a belief of thestate of the vehicle using the first head of the measurement modelsubject to measurement noise and relates 250 a the estimate of the statewith the belief on the state of the vehicle subject to estimation noiseusing the second head of the measurement model. Using the relatedmeasurements and estimates 255 a, the method updates 260 a recursivelyparameters of the probabilistic distribution of the state of the vehicleto produce the belief 265 a on the state of the vehicle.

FIG. 2B shows a probabilistic system 200 for tracking a state of avehicle using unsynchronized cooperation of information received fromsatellite signals 209 transmitted by a navigation satellite system(GNSS) and information transmitted over radio frequency (RF) channel 239according to some embodiments. The probabilistic system includes a GNSSreceiver 220 for receiving the satellite signals 209. In one embodiment,the signals include code signals, carrier phase signals, time stamps,navigation messages, and observation messages needed to determinevarious delays and disturbances affecting the satellite signals. Forinstance, the navigation message can include parameters needed todetermine atmospheric delays. For example, if the Klobuchar atmosphericdelay model is used, the navigation message includes parameters fordetermining an ionospheric time delay according to the ionosphere'seffect on the signal propagation determined by the atmospheric delaymodel. In other embodiments, such messages can include parameters fordetermining the ionospheric delay according to the SBAS delay model.

The system includes a memory 280 that stores 281 a probabilisticmulti-head measurement model relating incoming measurements with thestate of the vehicle. In some embodiments, the multi-head measurementmodel consists of two heads. The first head relates measurements of thesatellite signals with a belief on the state of the vehicle subject tomeasurement noise. The second head relates an estimation of the state ofthe vehicle with the belief on the state of the vehicle subject toestimation noise. The memory also stores instructions 283 on how todetermine such relation, and the memory stores 284 instructions storesto execute a probabilistic filter according to some embodiments of theinvention. The memory also stores 285 a probabilistic motion modelrelating a previous belief on the state of the vehicle with a predictionof the state of the vehicle according to the motion model. For example,the motion model can be a vehicle dynamics model, a constantacceleration model, a constant position model, a coordinated turn model,a Singer model, a kinematic vehicle model, a dynamic vehicle model, or acombination of different models.

In some embodiments, relating the measurements of the satellite signalswith a belief on the state and relating the estimation of the state withthe belief of the state is done by inserting said quantities into thefirst and second head, respectively, of the measurement model.

The GNSS receiver is operatively connected 250 to a GNSS measurementmodule, which, after determining the above-mentioned delays anddisturbances, determines the measurements of the satellite signals andthe associated measurement noise. For instance, in one embodiment themeasurement noise is determined based on a nominal measurement noisestored in memory 282 scaled with a function of the elevation angle ofthe satellite.

The probabilistic system 200 includes an RF measurement module 240 thatis operatively connected 250 to an RF receiver 260 that receivesinformation 239 transmitted over an RF channel. In some embodiments, theinformation includes a first moment of the state of the vehicle. Inother embodiments, the information includes the first moment and thesecond moment of a vehicle. In other embodiments, the informationincludes higher-order moments to form a general probability distributionof the state of the vehicle. In other embodiments, the informationincludes data indicative of an estimation of the state of the vehicleand the estimation noise, e.g., as samples, first and second moment,alternatively including higher-order moments. In yet other embodiments,the time of receiving the information is different from the time theinformation was determined. For example, in some embodiments, an activeremote server includes instructions to determine first and secondmoments, and the execution of such instructions, possibly coupled with acommunication time between the remote server and the RF receiver. Tothis end, in some embodiments, information 239 includes a timestamp ofthe time the first and second moments were determined.

In various embodiments, the GNSS measurement module and the RFmeasurement module produce their estimations that relate to the beliefon the state of the vehicle at a current instance of time. For example,the GNSS signals are received and upon receiving the signals, aprocessor 230 determines 231 measurements at a current time step.However, in some implementations, the RF measurement module receives thefirst and second moments of the state of the vehicle determined for aprevious time step, e.g., when said instructions of remote server taketime to execute. This leads to the determined measurement and receivedfirst and second moments do not correspond to the same time instant.

To address this issue, using a motion model, some embodiments relate thefirst and second moments of the state of a previous time step with abelief of the state in a current time step in time to determine theestimation of the state indicative of the belief on the state of thevehicle at a current instance of time. For example, one embodimentpropagates the first and second moments of the state of the vehicle intime till a current instance of time using a model of time propagation,for example, the motion model stored in memory, of the vehicle. Examplesof the model of time propagation include a Singer model, a constantacceleration model, and a single-track vehicle model. Using both momentsin time propagation is advantageous over propagating just a first momentbecause the propagation of uncertainty, hence how much the first momentshould be trusted when updating the belief of the state, is taken intoaccount. In other embodiments, the time propagation is done by acombination of a motion model of the vehicle and the measurements of thevehicle.

Some embodiments are based on a recognition that regardless of the timeinstance of receiving the first and the second moments of the state ofthe vehicle, there is a need to ensure that the estimation noise isconstructed in such a way that when updating the parameters ofprobabilistic distribution with the estimate of the state subject to themeasurement noise, the probability distribution of the belief of thestate converges to the probability distribution of the received secondmoment of the state. Some embodiments are based on the realization thatthe convergence property can be ensured by balancing covariances of thereceived second moment and the covariances of the probabilisticdistribution of the current belief of the state. In other words, theestimation of the state and associated measurement noise is determinedsuch that the balancing preserves convergence.

In some embodiments, processor 230 executes a probabilistic filter 232that is used to update the parameters of probabilistic distribution anddetermining estimates 233 of the state of the vehicle.

Some embodiments are based on understanding that it may be tempting todirectly set the parameters of probabilistic distribution to match thoseof the received first and second moment. However, enforcing theconvergence property is advantageous because it ensures that from thevehicle side, the same probabilistic filter can be used to update theparameters of the probabilistic distribution. Besides, since the vehiclemay process additional measurements, for example, from additionalonboard sensors of the vehicle, using the received first and secondmoments directly can cause discontinuities in the updated stateestimates of the vehicle. When using the estimates for controlling avehicle, such discontinuities can be detrimental because continuity ofcontrol is lost.

FIG. 2C shows a flowchart of an example of a method for determining 240a the estimates according to some embodiments. First, based on thereceived information indicative of the estimation of the state of thevehicle and the estimation noise, the method determines 210 c adistribution of the estimate of the state of the vehicle. For example,the method determines the distribution by inserting the first and secondmoment into a Gaussian probability density function. Next, the methoddetermines 220 c a distribution of the belief of the state of thevehicle. For instance, the method inserts the parameters of theprobability distribution of the state of the vehicle, wherein theprobability distribution is assumed to follow a predefined shape. Forinstance, for a Gaussian distribution, the parameters of the probabilitydistribution are the first and second moment. For instance, assuming astudent-t distribution the parameters are the first moment, scale, anddegree of freedom. Using the distribution 215 c of the estimation of thevehicle and the distribution 225 c of the belief of the state, themethod determines a difference between the distributions 215 c and 225c. Using the determined difference 235 c, finally, the method determines240 c a covariance of the estimation noise.

The determining 230 c difference can be done in several ways. Forexample, for a Gaussian distribution, the difference can be defined asthe amount of scaling the second moment and shifting the first moment toachieve the same distribution. In some embodiments, the difference isdetermined by the well-known Kullback-Leibler divergence.

Some embodiments are based on the recognition that typically the firsttwo moments describe a distribution well, even though the underlyingdistribution is not Gaussian. In some embodiments, this recognition isutilized in determining 240 c the covariance of the estimation noise asa weighted combination of the second moment received by the RFmeasurement module and the second moment of the probabilisticdistribution of the state of the vehicle. In other embodiments, theestimate of the state of the vehicle is determined as a weightedcombination of the belief of the state of the vehicle and the receivedfirst moment of the estimation of the state of the vehicle, wherein theweighting includes the covariance of the estimation noise.

FIG. 2D shows an example of a method for determining 240 c the first andsecond moment to ensure that for Gaussian-assumed distributions, thebelief on the state of the vehicle converges to the estimation of thestate of the vehicle according to some embodiments. Using thedistribution of the estimate 215 c and the distribution of the belief225 c, the difference of the distributions is defined through the Kalmangain, which is the gain matrix needed to move the Gaussian-assumeddistribution from one distribution to another distribution. The methoddetermines 210 d, starting from the distribution of the belief 225 c,the Kalman gain as a function of the covariance of the estimation noisethat moves the distribution of the belief to the distribution determinedby the received first and second moment. E.g., the Kalman gain K movesthe covariance according to Σ_(k)=(I−K)(Σ_(k−1)+Q_(k)). Based on theKalman gain 215 d, the covariance of the estimation noise is determined220 d by solving the matrix including the Kalman gain. E.g. oneembodiment solves for the covariance R_(k), of the estimation noise bythe following set of equations:

$\sum\limits_{k}{= {\left( {I - K} \right)\left( {\sum\limits_{k - 1}{+ Q_{k}}} \right)}}$${K = {\left( {\sum\limits_{k - 1}{+ Q_{k}}} \right)\left( {{\sum\limits_{k - 1}{+ Q_{k}}} + R_{k}} \right)^{- 1}}},$which results in R_(k)=−(−Σ_(k) ⁻¹+(Σ_(k−1)+Q_(k))⁻¹)⁻¹, i.e., thecovariance of the estimation noise The estimation of the state of thevehicle is determined by adjusting the belief on the state of thevehicle in response to the Kalman gain and a difference of the receivedfirst moment and the belief of the state of the vehicle. E.g, theestimation is determined asz_(k)=μ_(k−1)+(Σ_(k−1)+Q_(k)+R_(k))(Σ_(k−1)+Q_(k))⁻¹(μ_(k)−μ_(k−1))

Some embodiments update the parameters of probabilistic distributionusing the first head and the second head of the measurement model. Forinstance, one embodiment updates the parameters of probabilisticdistribution using a KF having the first head of the measurement modelto adjust the parameters. Another embodiment acknowledges that themotion model or the measurement model is nonlinear for some states ofthe vehicle. To this end, some embodiments employ nonlinear KFs, such aslinear-regression KF, to update the parameter of the probabilisticdistribution. Linear-regression KFs are filters that determine the firstand second moment by having the moment integrals including m^(o)+

and Σ^(oo)=

evaluated at a set of integration points P={(w^((i)),ε^((i)))}_(i+1)^(K(I)).

Some embodiments recognize that one purpose of the second head ofmeasurement model is to allow the update of parameters of probabilisticdistribution using the same principles as done when using the first headof the measurement model. Some embodiments recognize that to update theparameters of probabilistic distribution using the estimation of thestate of the vehicle determined using the received first and secondmoment, the second head of the measurement module must ensure certainproperties to ensure that the convergence of the belief of the state ofthe vehicle converges to the estimation of the state of the vehicle. Forinstance, in one embodiment the second head of the measurement modelincludes the estimated state of the vehicle and the belief on the stateof the vehicle determined during a previous iteration as a differenceweighted with the covariance of the estimation noise. For instance, inone embodiment the second head of the measurement model isz_(k)=χ_(k)+r_(k), wherein z_(k) is the estimated of the state of thevehicle, χ_(k) is the belief of the state of the vehicle, and r_(k) isthe estimation noise, wherein k is the time step in the recursion.

Some embodiments are based on the recognition that in order to updatethe parameters of probabilistic distribution using a probabilisticfilter, the probabilistic filter should be able to update the parametersby accounting for the fact that a part of the state of the vehicle isreal-valued and one part is integer-valued. This is because certainparts, for example, the position of the vehicle is included in the stateand real-valued, whereas the ambiguity is included in the state and isinteger-valued.

To this end, some embodiments employ a probabilistic filter that updatesthe parameters of probabilistic distribution by solving a recursivemixed-integer weighted least squares problem according to

$\underset{{x_{k} \in {\mathbb{R}}^{m}},{n_{k} \in {\mathbb{Z}}^{n}}}{argmin}{{{y_{k} - {H_{k}x_{k}} - {G_{k}n_{k}}}}_{R_{k}^{- 1}}^{2}.}$

Some embodiments are based on the understanding that a particle filteris a filter that can solve mixed-integer estimation problems withouthaving to resort to optimization methods. Other embodiments understandthat in order to not having to resort to optimization methods, particlefilters need a large number of particles, which can be computationallyprohibitive. To this end, some embodiments solve the recursivemixed-integer weighted least squares problem by employing amixed-integer extended KF. Other embodiments solve mixed-integerlinear-regression KFs.

FIG. 3A shows a flowchart of a method of an exemplar implementation forstate estimation of a vehicle wherein the method receives 310 ameasurements and estimates of the states including the multi-headmeasurement model. Each carrier signal includes a carrier phaseambiguity as an unknown integer number of wavelengths of the carriersignal traveled between the satellite 101, 102, 103, or 104 and thereceiver 130. Next, the method retrieves 322 a from a memory a motionmodel relating a previous state of the vehicle to a current state of thevehicle and the multi-head measurement model relating measurements ofthe carrier and the code signals to the current belief of the state ofthe vehicle using the carrier phase ambiguities of the carrier signalsand the estimation of the state of the vehicle to the belief of thestate of the vehicle. Both models, i.e., the motion model and themeasurement model, are probabilistic. For example, the motion model is aprobabilistic model subject to process noise, and the measurement modelis a probabilistic model subject to measurement noise.

The method then determines 320 a a set of possible combinations ofinteger values 325 a of the carrier phase ambiguities consistent withthe measurements of the carrier and the code signals according to one orcombination of the motion model and the measurement model 322 a withinbounds defined by one or combination of the process noise and themeasurement noise. This step is based on understanding that instead ofattempting to determine the carrier phase ambiguities to perform a stateestimation, it is beneficial to determine and test different possiblecombinations of the carrier phase ambiguities for the state estimation.In such a manner, the best carrier phase ambiguities can be selectedusing the probabilistic model better reflecting the nuances of the stateestimation.

To that end, the method executes 320 a a set of state estimatorsdetermining states of the vehicle by jointly using the motion model andthe measurement model 322 b. Each state estimator includes itscorresponding combination of the integer values of the carrier phaseambiguities to determine a joint probability distribution 335 a of thestate of the vehicle with respect to the motion model and themeasurement model 322 a. In such a manner, the combinations of theinteger values of the carrier phase ambiguities can be evaluatedprobabilistically, because the measurement models of at least somedifferent state estimators include different combinations of integervalues of the carrier phase ambiguities selected from the set ofpossible combinations 325 a. Next, the method determines 340 a the stateof the vehicle using a state estimator with the highest jointprobability of the state of the vehicle according to the measurements ofthe carrier and the code signals and the estimation of the state of thevehicle using the multi-head measurement model.

To that end, some embodiments are based on the realization thatestimation of the range of possible integer values of carrier-phaseambiguities as well as the selection of the integer values ofcarrier-phase ambiguities from that range can be done probabilisticallyusing consistency of the motion and the measurement models with respectto a probability density function (PDF) of the noise of the first headof the measurement model and the noise of the second head of themeasurement model.

FIG. 3B shows a flowchart of a method for determining 320 a the possiblecombinations of integer values of the carrier phase ambiguitiesaccording to some embodiments. The method can be implemented using aprocessor. For at least one state of the vehicle consistent with theprocess noise of the motion model, the method samples 310 b float values311 b of the carrier phase ambiguity, for at least one satellite, on aPDF of the measurement noise centered on a noiseless fit of the carrierphase ambiguities, the state, the measurements of the carrier and codesignals into the measurement model and the estimation of the state.

In some embodiments, the PDF of the measurement noise is predeterminedbased on, e.g., characteristics of the GNSS receiver. As used herein,the noiseless fit is the fit of the carrier phase ambiguities, theposition, and the measurements of the carrier and code signals, when theobtained measurements of the carrier and code signals are assumed to becorrect. That is, the measurements of the carrier and code signals areassumed to be correct and the remaining errors are due to the carrierphase ambiguity. In such a manner, the noiseless fit places the PDF in aposition that the sampling on the PDF deemphasizes the probabilisticnoise of the motion and the measurement models and emphasizes the effectof the carrier phase ambiguities on the position estimation. In otherwords, the errors in the fit to the measurement model after insertingcarrier phase ambiguities, the position, and the measurements of thecarrier and code signals into the measurement model, are due to theerror in the carrier phase ambiguities.

Then, the method forms 320 b a union of the sampled float values, toproduce a union 321 b that contains all of the sampled float values;discretizes 310 b the union of float values in an integer basis toproduce possible integer values 331 b of the carrier phase ambiguity forthe set of GNSS satellites. Finally, the method uses the possibleinteger values 331 b of carrier-phase ambiguities for all satellites, toproduce 340 b a set of possible combinations 325 a of integer values.

FIG. 3C shows a flowchart of an exemplar implementation of method 310 bthat samples the float values according to one embodiment. However, thesampling 310 b the float values is implemented in several ways bydifferent embodiments. The method of FIG. 3C samples 309 c float valuesconsistent with the motion model and its process noise. The methoddetermines 311 c consistency with the measurements of the code andcarrier phase signals, by inserting sampled carrier phase ambiguities,the estimated state, and the measurements of the carrier and the codesignals into the measurement model. Based on the consistency with themeasurement, the method corrects 312 c each sampled float value as afunction of the process noise and the measurement noise; updates 313 cthe probability of each ambiguity based on the consistency with themeasurements after correction; and prunes 314 c the corrected sampledfloat values of carrier phase ambiguities to preserve the float valuesof carrier-phase ambiguities with probabilities of fitting into themeasurement model above a threshold.

FIG. 3D shows a schematic illustrating some principles employed by someembodiments. Specifically, some embodiments are based on the realizationthat the finite number of possible integer values of carrier phaseambiguity allows determining 340 d different combinations of thosepossible integer values for tracking the state of the vehicle. Such arealization allows replacing 380 d the evaluation 310 d of the carrierphase ambiguity for estimating 320 d the state 330 d of the vehicle withthe evaluation 360 d of different state of the vehicle determined 350 dusing different the combinations 340 d of the carrier phase ambiguities.This replacement is advantageous because the probabilistic nature of themotion of the receiver is better equipped to test the position than totest the derivative of the position, such as the carrier phaseambiguity. In such a manner, the best state 370 d selected using theprobabilistic nature of the motion of the vehicle automaticallyindicates the corresponding combination of the carrier phase ambiguityused to determine such a state 370 d.

Some embodiments are based on the recognition that a particle filter canbe computationally prohibitive, and that for computational reasons, aKalman-type filter embedding an optimization solver can be advantageous.

FIG. 4 shows a flowchart of a method 260 a for executing a probabilisticfilter to update parameters of the probabilistic distribution of a stateof a vehicle when the parameters are the first and second moment of thedistribution according to one embodiment. The method receives 410 ameasurements and estimates of the states including the multi-headmeasurement model. Each carrier signal includes a carrier phaseambiguity as an unknown integer number of wavelengths of the carriersignal traveled between the satellite101, 102, 103, or 104 and thevehicle 130. Next, the method retrieves 422 a from a memory a motionmodel relating a previous state of the vehicle to a current state of thevehicle and the multi-head measurement model relating measurements ofthe carrier and the code signals to the current belief of the state ofthe vehicle using the carrier phase ambiguities of the carrier signalsand the estimation of the state of the vehicle to the belief of thestate of the vehicle. Both models, i.e., the motion model and themeasurement model, are probabilistic. For example, the motion model is aprobabilistic model subject to process noise, and the measurement modelis a probabilistic model subject to measurement noise. Using themulti-head measurement model, measurements 255 a, motion model, and theparameters of probability distribution determined during a previousiteration, the method executes 420 a a KF that updates the first andsecond moment of the probability distribution, wherein the updated firstmoment of the ambiguity contained in the state is real-valued. Next, themethod solves 430 a a weighted least squares optimization problem thatfixes the ambiguities to have integer values, wherein the optimizationproblem cost function is a squared Euclidean norm of the deviation ofthe first moment of the state to the real-valued estimates 425 adetermined by the Kalman filter 420 a. Using the resulting integerambiguities 435 a, the method then executes 440 a a Kalman filterinitialized with the real-valued part of the first moment and theinteger-valued part of the first moment, resulting in updated 445 aparameters of the probabilistic distribution.

In some embodiments, the KF is an extended KF, wherein the nonlinearparts of the motion and measurement model are linearized around thecurrent belief of the state. In other embodiment, the KF is alinear-regression KF, for example, an unscented KF, a cubature KF, or asmart-sampling KF. Linear-regression KFs avoid the linearization aroundthe current state estimate as in the extended KF and are generally moreaccurate but more computationally complex. Instead of linearization,linear-regression KFs solve the involved moment integrals by a set ofweighted integration points and can be used to also determine otherparameters of probabilistic distributions, e.g., higher-order moments.Such higher-order moments can be useful in situations when the first andsecond moments do not sufficiently well represent the underlyingdistribution.

The received first and the second moment from the RF receiver can beinformation received from multiple sources. For example, in someembodiments, a passive road-side unit (RSU) equipped with a cameradetermines parts of the state of the vehicle, e.g., position andvelocity using computer vision algorithms with high accuracy. In otherembodiments, the RSU is equipped with a range sensor, e.g., radar orlidar, which directly measures the position and velocity of the vehiclerelative to the RSU. When parts of the state can be resolved directlywith high accuracy using direct sensing, an RSU can then determine theremainder of the state of the vehicle, and transmit that to the vehicle.

FIG. 5A shows a traffic scenario illustrating a scenario according tosome embodiments. Some embodiments are based on a recognition that anactive remote server is located in the cloud as an edge computingdevice, and to establish communication among vehicles and the cloud,information transfer between cloud and vehicle needs to go through thecore infrastructure network and RSU as shown in FIG. 5A, where thecommunication between vehicle V1 510 a and cloud 520 a needs to gothrough RSU 530 a and core network 540 a.

Other embodiments recognize that having computations in the cloud causeslarge communication delays. In such situations, having an edge computingdevice directly in the RSU is advantageous.

Some embodiments are based on a recognition that using remote servers incommunication with vehicles necessitates using different clocks fordifferent devices, and synchronization among such clocks is necessary.

The clock of the vehicle can be synchronized to the clock of the GNSS incommunication with the GNSS receiver of the vehicle. However, someembodiments are based on a recognition that individual synchronizationof the clock of the vehicle to a clock of the GNSS does not guaranteethe synchronization of the vehicles in the vehicular communicationnetworks due to different clock offsets errors that different vehiclesmight have. In addition, the GNSS synchronization can be negativelyaffected by the multipath of the satellite signals in the urbanenvironment. In addition, some vehicles in the vehicular communicationnetwork may not have a GNSS receiver.

Different standards provide protocols for mutual synchronization of theclocks. For example, IEEE has been developing precision time protocol(PTP) based synchronization standards. However, those protocols requireinformation exchange between synchronizing devices, which in the contextof a versatile vehicular communication network can be impractical.

Some embodiments are based on the realization that vehicularcommunication network has special requirements on participatingvehicles. Unlike a node in other networks, a vehicle in a vehicularcommunication network periodically announces its presence. For example,in IEEE Dedicated Short Range Communications (DSRC) for Wireless Accessin Vehicular Environments (WAVE), a vehicle is required to transmit aheartbeat message every 100 ms to announce its presence to neighboringvehicles. The attributes of the heartbeat messages include one or acombination of temporary ID, time, latitude, longitude, elevation,positional accuracy, speed and transmission, heading, acceleration,steering wheel angle, brake system status, and vehicle size.

Some embodiments are based on the realization that this heartbeatmessage includes necessary synchronization data and therefore, can beused for synchronization. This approach reduces network traffic andmitigates the interference. With synchronization data transmittedautomatically, a vehicle can achieve silent synchronization withoutmessage exchange executed in conventional synchronization methods.

For example, some embodiments are based on the realization that a clockof the vehicle can be synchronized to clocks of other vehicles based ona trilateration of information received in multiple heartbeat messagesunder the assumption that the clock offset of the vehicle and locationof the vehicle is unknown. This is because the trilateration of thevehicle can be performed with two kinds of methods. One method usesdistances that the light travels between the transmission and receipttime of the heartbeat message that is a function of the unknown clockoffset. Another method uses distances between locations of the multiplevehicles and the time of transmitting the heartbeat messages and theunknown location of the vehicle at a particular time. By comparing thesetwo kinds of distances it is possible to concurrently determine theunknown clock offset and unknown location of the vehicle at a particulartime.

FIG. 5B shows the synchronization message exchange in IEEE PrecisionTime Protocol (PTP) based synchronization. Such a synchronizationrequires at least two message exchanges between master and slave. Masterfirst sends a Sync message to slave at time T₁. Slave receives Syncmessage at time T₂. Even Sync message contains the timestamp T₁, formore precise synchronization, a master may send a Follow_Up message thatcontains the precise timestamp T₁ to the slave. After processing Syncmessage and Follow_Up message received from the master, the slave sendsa Delay_Req message to the master at time T₃. Once master receives theDelay_Req message at time T₄, it sends back a Delay_Resp message thatcontains timestamp T₄ to the slave. Using four timestamps T₁, T₂, T₃ andT₄, the slave computes message propagation time and clock offset, andtherefore, synchronizes its clock to master's clock. For example, for asymmetric link, the propagation time t_(p) and clock offset t_(o) can becomputed as

$\begin{matrix}{t_{p} = \frac{\left( {T_{4} - T_{1}} \right) - \left( {T_{3} - T_{2}} \right)}{2}} & (1)\end{matrix}$ and $\begin{matrix}{t_{o} = \frac{\left( {T_{2} - T_{1}} \right) + \left( {T_{3} - T_{4}} \right)}{2}} & (2)\end{matrix}$

In some embodiments, the parameters of the probabilistic distribution ofthe state of the vehicle together with the GNSS measurements aretransmitted to the remote server.

FIG. 5C shows a probabilistic system 200 for tracking a state of avehicle using unsynchronized cooperation of information received fromsatellite signals 209 transmitted by a navigation satellite system(GNSS) and information transmitted over radio frequency (RF) channel 239according to some embodiments. The system includes a transmitter 510 cthat transmits its parameters of probabilistic distribution andsatellite measurement signals 511 c to the remote server.

In other embodiments, multiple vehicles are transmitting theirparameters of probabilistic distributions and the GNSS measurementsignals to the remote server. The parameters include the first momentand the second moment of the probabilistic distribution of the state ofthe vehicle. Such information sending can be beneficial when the remoteserver, e.g. located in the RSU or in the cloud, does not haveadditional sensing capabilities. In such cases, the remote server canuse the information received from multiple vehicles to merge suchinformation and therefore improve the estimation of the state of eachvehicle and transmit the first and second moment to the vehicles. Inother words, the probabilistic system of the vehicle transmits itsparameters of the probabilistic distribution together with satellitemeasurements and receives the first and second moment of the state ofthe vehicle in response to the transmission. In some embodiments, theremote server forms an augmented state based on the state of themultiple vehicles.

In some embodiments, the augmented state is a union of states ofmultiple vehicles. For example, the augmented state includes theposition and velocity of the vehicles, the carrier phase ambiguities,and residual bias states, e.g., ionospheric residual delays. Forvehicles sufficiently close to each other, for example, less than 3-10km apart, the ionospheric delay can be considered the same for differentvehicles. For example, referring to FIG. 5D, vehicles inside the region510 d have very similar ionospheric delays. Hence, in some embodiment,the dimension of the augmented state is smaller than the sum of thestate of all vehicles, because such ionospheric delays are the same formultiple vehicles.

In the majority of GNSS applications, the residual biases are suppressedthrough single or double differences operations and the focus of theseapplications is to recover the integer ambiguities that form a part ofGNSS measurements. However, some embodiments are based on therealization that the residual biases are still affecting the GNSSmeasurements due to the nature of the physics of propagation of GNSSsignals. However, these biases cannot be determined from a single GNSSreceiver alone but using augmentation of measurements of multiplemeasurements from multiple satellites measuring multiple receivers. Thisis because the integer ambiguity is unique for each satellite-vehiclecombination, but the residual biases are not. For example, theionospheric delay is included as a residual bias in the state, and thisis the same for vehicles being measured in close vicinity of each other.To that end, to recover these residual biases, the measurement noiseshould be estimated to determine the correlation between measurements ofmultiple vehicles of satellite signals, which is a task very differentfrom the task of integer ambiguity estimation.

Some embodiments are based on the realization that such a task ofestimating the residual bias is difficult to address at the level of anindividual vehicle because there are many unknowns. This is becausewhile each vehicle receives multiple GNSS signals from multiplesatellites, and the residual biases are similar for the different GNSSsignals, the ambiguities are not. Hence, even for a single receiver,using more measurements does not help significantly in terms ofestimating the residual bias.

On the other hand, when using multiple measurements of multiplevehicles, there is a cross-correlation between GNSS measurements ofdifferent vehicles, because such measurements arise from the samesatellite.

FIG. 6A shows a flowchart of a recursive method 600 for cooperativestate tracking of multiple vehicles according to some embodiments. Themethod is executed by a processor of the remote server, e.g., an edgedevice in an RSU or in the cloud. The remote server receives 610 amultiple transmissions 605 a from multiple vehicles. Each transmission605 a from a vehicle includes the measurements of satellite signalsreceived at the vehicle and parameters of the probabilistic distributionof the state of the vehicle.

The remote server fuses the states of the multiple vehicles defined bythe parameters of the corresponding probabilistic distributions to form620 a an augmented state 625 a of the multiple vehicles. In oneembodiment, the method 600 just concatenate all state together in avector, matrix or a tensor. However, in some other embodiments, someresidual biases of a neighboring vehicle are identical and the method600 removes the duplication forming 620 a the augmented state 625 a as aunion of the states of multiple vehicles, such that dimensions of theaugmented state is smaller than a sum of dimensions of the state of eachof the multiple vehicles.

In addition, method 600 fuses 630 a the measurements of satellitesignals of the multiple vehicles into an augmented measurement of theaugmented state subject to augmented measurement noise 635 a. Someembodiments are based on a realization of dependency of measurementnoise of different vehicles. Hence, the augmented measurement noise 623a is defined by a non-diagonal covariance matrix with non-zerooff-diagonal elements, each non-zero off-diagonal element relatingerrors in the measurements of a pair of corresponding vehicles.

Method 600 executes 640 a a probabilistic filter updating the augmentedstate 645 a based on the augmented measurement subject to the augmentedmeasurement noise. Due to one or a combination of the redundancy ofstate variables and cross-correlation of measurement noise, the accuracyof the updated augmented state 645 a for each of the individual vehiclescan be greater than the accuracy of the state of the vehicle determinedindividually.

To avoid discontinuity of control, method 600 fuses 650 a the parametersof the probabilistic distribution of the state of at least some of themultiple vehicles with a corresponding portion of the updated augmentedstate 645 a to output the fused parameters 655 a of the probabilisticdistribution of the state of at least some of the multiple vehicles. Thefused parameters 655 a can be transmitted to the vehicles, used for thecontrol of the vehicle, and for other purposes taking advantage of stateestimation.

The execution 640 a of the probabilistic filter can be performed in anumber of ways using different measurement models such as a multi-heador single-head measurement model and using various motion models, suchas a constant velocity motion model, a constant acceleration motionmodel, a Singer model, a kinematic vehicle model, and a dynamic vehiclemodel. Examples of the probabilistic filter include a particle filter ora mixed-integer Kalman filter.

Some embodiments are based on understanding that the state of multiplevehicles can be tracked jointly using principles of probabilisticestimation similar to the principles of probabilistic tracking of anindividual vehicle. At a first glance, such joint tracking does not makesense, because it increases the dimensionality of the tracking problemwithout immediately apparent benefits. This is because the integerambiguities of different vehicles, which is the focus of most GNSSapplications, would be independent of each other in such a collectivetracking, and thus, the collective tracking would not help to resolvethe ambiguities.

However, some embodiments are based on the realization that during acollective state estimation of multiple vehicles, the measurement noiseimposed on the collective state, i.e., referred to in this disclosure asan augmented state, of multiple vehicles does indeed have useful crosscorrelation between the noise of the measurements of the states ofdifferent vehicles that can be explored to reduce the uncertainty on theestimation of the augmented state. E.g., when a satellite measures twovehicles, there is a direct cross correlation between the twomeasurements, because they arise from the same source and therefore havesimilar noise properties.

Some embodiments are based on understanding that the augmented stateestimation can correct or at least improve the state estimationperformed by an individual vehicle. However, there is a need to softlymerge the results of augmented and individual state tracking in order toavoid the discontinuity of control of the vehicle. This is because anindividual vehicle can update at faster rates and have additionalonboard sensors, e.g., inertial measurement unit and wheel encoders,which can provide different information. Hence, just using the resultsfrom the augmented state directly can result in loss of continuity ofestimation, which can be detrimental to vehicle safety. To that end,some embodiments fuse the parameters of the probabilistic distributionof the state of an individual vehicle with a corresponding portion ofthe updated augmented state, as contrasted with the updating the stateof the individual vehicle itself. In such a manner, the resultingestimate is a tradeoff between the additional information, such ascorrelated measurement noise and similarity of residual biases used bythe server, with the smoothness provided by always using individualvehicle estimates.

Additionally or alternatively, some embodiments appreciate the need forreducing the computational burden of such a high dimensionalprobabilistic filter tracking the augmented state. For instance, a stateof a vehicle can have a state dimension of up to dimension 100,depending on how many residual biases are estimated and how manysatellites currently measure the vehicle. On the other hand, there canbe hundreds of vehicles in the vicinity of each other, and in the worstcase, the dimension of the augmented state can be tens of thousands.Hence, the computations involved in the remote server are morecomputationally demanding, and especially the solution of amixed-integer estimation problem where numerous integer ambiguities needto be fixed becomes prohibitive. Some embodiments recognize that inorder to solve a mixed-integer least squares problem for the augmentedstate, mixed-integer Kalman filters should be used.

Some embodiments are based on the understanding that when designingrecursive mixed-integer estimators, it implies the need for solving amixed-integer problem of ever-increasing size since a current estimateof the state depends on the entire history of integer values. Hence, tosolve such a large-scale estimation problem, various relaxations need tobe made, because otherwise there is an exponential growth of complexitywith time.

One embodiment is based on the understanding that a way to relax themixed-integer estimation problem, instead of having an integer ambiguitybe determined by optimization from a real-valued ambiguity andsubsequently update parameters of distribution based on said integerambiguity, it is beneficial to determine parameters of two probabilisticdistributions recursively and jointly, wherein one distribution onlyworks with real-valued state, and one distribution models the integerambiguities. This is because if using two distributions, real-valuedestimates are not mixed with mixed-integer estimates and the truedistribution of real-valued state is therefore not contaminated.

FIG. 6B shows a flowchart of a recursive method for updating 620 a theparameters of the probabilistic distribution of augmented state at theremote server according to some embodiments. Using the measurements 605a of multiple transmissions, the method updates two sets of parametersof distributions jointly, one unconstrained using real values and oneconstrained to having the ambiguities as integer values. First, using amotion model and a measurement model 622 b, the method determines 620 bthe one-step prediction of parameters of the probability distribution ofthe unconstrained augmented state. Next, the method updates 630 b theparameters of the probability distribution of the unconstrainedaugmented state using the satellite measurements

Using the real-valued parameters of ambiguities, the method fixes 640 bthe ambiguities, and using the constrained ambiguities and themeasurement model 622 b, the method constrains 619 b the augmented stateby determining a relationship between the satellite measurements and theaugmented state having the ambiguities as integers, resulting in aconstrained relation. Next, the method determines 639 b a one-stepprediction of the parameters of the probabilistic distribution ofconstrained augmented state, and finally updates 649 b the parameters ofthe probabilistic distribution of constrained augmented state.

The respective one-step predictions can be done using the motion modeland the measurement model jointly using Kalman filter time updates. Theupdating of the parameters can similarly be done using the Kalmanfilter. For example, using an extended Kalman filter, some embodimentsperform the time updating, i.e., one-step prediction as

m_(k❘k − 1)^(x^(f)) = f_(k − 1)(m_(k − 1❘k − 1)^(x^(f))),m_(k❘k − 1)^(n^(f)) = G_(k)m_(k − 1❘k − 1)^(n^(f)),m_(k❘k − 1)^(y) = h_(k)(m_(k❘k − 1)^(x^(f))) + G_(k)m_(k❘k − 1)^(n^(f)),${\sum\limits_{k❘{k - 1}}^{x^{f}x^{f}}{= {{F_{k = 1}^{f}{\sum\limits_{{k - 1}❘{k - 1}}^{x^{f}x^{f}}\left( F_{k - 1}^{f} \right)^{\top}}} + W_{k - 1}}}},$${\sum\limits_{k❘{k - 1}}^{n^{f}n^{f}}{= {\sum\limits_{{k - 1}❘{k - 1}}^{n^{f}n^{f}}{+ V_{k - 1}}}}},$${\sum\limits_{k❘{k - 1}}^{yy}{= {{H_{k}^{f}{\sum\limits_{k❘{k - 1}}^{x^{f}x^{f}}\left( H_{k}^{f} \right)^{\top}}} + {G_{k}{\sum\limits_{k❘{k - 1}}^{n^{f}n^{f}}G_{k}^{\top}}} + R_{k}}}},$${\sum\limits_{k❘{k - 1}}^{x^{f}y}{= {\sum\limits_{k❘{k - 1}}^{x^{f}x^{f}}H_{k}^{\top}}}},$${\sum\limits_{k❘{k - 1}}^{n^{f}y}{= {\sum\limits_{k❘{k - 1}}^{n^{f}n^{f}}G_{k}^{\top}}}},$${{F_{k - 1}^{f} = \frac{\partial{f_{k - 1}(x)}}{\partial x}}❘}_{x = m_{{k - 1}❘{k - 1}}^{x^{f}}},$${{H_{k}^{f} = \frac{\partial{h_{k}(x)}}{\partial x}}❘}_{x = m_{k❘{k - 1}}^{x^{f}}}.$

Some embodiments constrain 640 b the ambiguity to be integer values bysolving the weighted least squares problem

${n_{k}^{I} = {\underset{n_{k} \in {\mathbb{Z}}^{n}}{argmin}{{n_{k} - m_{k|k}^{n^{J}}}}_{({(\sum_{k|k}^{n^{f_{n}f}})}^{- 1}}}},$wherein the weight is the updated second moment of the unconstrainedaugmented state, and wherein m_(k|k) ^(n) ^(f) is the first moment ofthe real-valued, i.e., unconstrained, ambiguity.

Some embodiments are based on the recognition that ambiguities have ajump behavior due to the presence of occasional cycle slips. Theambiguities are likely to remain constant over long periods of timebefore single ambiguities suddenly jump to new integer values during acycle slip event. Some embodiments detect the cycle slip by modeling theambiguity time evolution, i.e., the ambiguity motion model 622 b as adiscrete random walk. In some embodiments, for an ambiguity n, the timeevolution of the ambiguity is modeled as the discrete random walk

${n_{k} = {n_{k - 1} + {\underline{v}}_{k - 1}}},$${\left. {\underline{v}}_{k - 1} \right.\sim{\mathcal{N}\left( {{v_{k - 1}❘0},V_{k - 1}} \right)}},$V_(k − 1) = diag(c_(k)σ_(jump)² + (1 − c_(k))σ_(stay)²),

Wherein σ_(jump) ² is a variance reflecting the width of the domain ofthe integer jump process, σ_(stay) ² is a regularization term, andc_(i,k) that determines whether a cycle slip has occurred.

In some embodiments, the difference between the satellite measurementsand the predicted satellite measurements is formed to determine whethera cycle slip has occurred. For example, one embodiment forms thedifference as δy_(k)=

[y _(k)−y _(k) ^(f)] and recognizes that between two consecutivetimestamps, the most likely event that causes a sudden change in saiddifference is due to cycle slip. Hence, the difference equalsδy_(k)=G_(k)δn_(k)⇔δn_(k)=(G_(k) ^(τ)G_(k))⁻¹G_(k)δy_(k), which meansthat if any dimension of the difference in ambiguity, δn_(k), issufficiently far away from the origin, determined by a threshold d, itis because of a cycle slip. In one embodiment the incidence c_(i,k) isdetermined as

$c_{i,k} = \left\{ {\begin{matrix}{1\ } & {{{if}{❘{\delta n_{i,k}}❘}} > d} \\0 & {otherwise}\end{matrix}.} \right.$

FIG. 6C shows a flowchart of a method 630 a for fusing multiple statesof the vehicle with the augmented state according to some embodiments.Since the state of the vehicles and the augmented state generally arenot of the same dimension, the states need to be transformed to a jointstate space that equally represents the different states. In oneimplementation, the method 630 a transforms 610 c the parameters of thestates of the vehicle to the same state space as the parameters of theaugmented state.

In one embodiment, for an augmented state distribution with the firstand second moments, p(χ^(g)|{circumflex over (χ)}^(g), Σ^(g), and stateof a vehicle as χ^(l), the transformation is defined byχ^(l)=T_(g→l)χ^(g)=LL^(τ)χ^(g), which means there are two sets of thefirst and second moments defined in the augmented state space,μ_(A)=T_(g→l) ^(τ){circumflex over (χ)}^(l),Σ_(A)=T_(g→l)^(τ)Σ^(l)T_(g→l),μ_(B)={circumflex over (χ)}^(g),Σ_(B)=Σ^(g). In oneembodiment, the state space is equal and such transformation need not bedone. Using the transformed parameters, the method fuses 620 c to forman updated set of parameters of the probabilistic distribution.

The fusing can be done in many ways. In one embodiment, the fusing 620 cof parameters is done as a weighted combination between the augmentedstate and the state of each vehicle. For example, one embodiment fusesthe first and second moment as a convex combination of moments,

x̂^(f) = w_(A)μ_(A) + w_(B)μ_(B),${P^{f} = {{w_{A}{\sum\limits_{A}{{+ {w_{A}\left( {\mu_{A} - {\hat{x}}^{f}} \right)}}\left( {\mu_{A} - {\hat{x}}^{f}} \right)^{\top}}}} + {w_{B}{\sum\limits_{B}{{+ {w_{B}\left( {\mu_{B} - {\hat{x}}^{f}} \right)}}\left( {\mu_{B} - {\hat{x}}^{f}} \right)^{\top}}}}}},$wherein the weights are determined by minimizing the determinant of thefused covariance,

${w_{A} = {\underset{w_{A}}{\arg\min}{\det\left( P^{f} \right)}}},$w_(B) = 1 − w_(A).In other embodiments, the weights are determined by minimizing the traceof the fused covariance

${w_{A} = {\underset{w_{A}}{\arg\min}{{Tr}\left( P^{f} \right)}}},$w_(B) = 1 − w_(A).In yet another embodiment, the weights are determined by setting theweights according to the ratio

${w_{A} = \frac{{{Tr}\left( \sum\limits_{A} \right)}^{- 1}}{{{Tr}\left( \sum\limits_{A} \right)}^{- 1} + {{Tr}\left( \sum\limits_{B} \right)}^{- 1}}},{w_{B} = {1 - {w_{A}.}}}$In yet a different embodiment, the fusing is done according to theweighted combinationP ^(f)=(w _(A)Σ_(A) ⁻¹ +w _(B)Σ_(B) ⁻¹)⁻¹,K _(A) =w _(A) P ^(f)Σ_(A) ⁻¹,K _(B) =w _(B) P ^(F)Σ_(B) ⁻¹,{circumflex over (χ)}^(f) =K _(A)μ_(A) +K _(B)μ_(B),

FIG. 6D shows an example of the overlapping of state space according tosome embodiments. In the left part of FIG. 6D, the state spaces aresimilar between the three different vehicles 610 d, 620 d, 630 d, andthe augmented state 640 d, that is, there is no transformation neededbetween the two. In the middle part of FIG. 6D, the state spaces arecompletely disjoint, whereas in the right figure there is overlapbetween the states.

The above-described fusion rules are conservative since they do notconsider the cross-covariance between the augmented state and the stateof the vehicle.

FIG. 6E shows a flowchart of a method 620 c for fusing the augmentedstate with the state of vehicles according to some embodiments. Ifnecessary, such as in the third case in FIG. 6D, the method uses thetransformed 610 e parameters to joint state space and a measurementmodel 615 e of satellite measurements to recursively update thecross-covariance between the state of vehicles, and state of the vehicleand augmented state. Finally, the method fuses 630 e the first andsecond moment according to the determined cross-covariance.

The recursive determining 620 e the cross-covariance can be done inmultiple ways. For example, one embodiment recognizes that if a Kalmanfilter is used, for example, an extended Kalman filter, alinear-regression Kalman filter, a mixed-integer extended Kalman filter,or a mixed-integer linear-regression Kalman filter, the cross-covariancecan be determined recursively at each time step k of estimation. This isbecause the probabilistic distribution in such a filter is representedby the first two moments in the form of a Gaussian distribution.

FIG. 6F shows a flowchart of a method 620 c for determining thecross-covariance according to one embodiment. Using a motion model and ameasurement model 615 f of the augmented state and the state of thevehicles, the method determines 610 f the Kalman gain of the involvedKalman filters. For example, one embodiment determines the Kalman gainfor the vehicles and the Kalman gain for the Kalman filter estimatingthe augmented state, wherein the Kalman gain is determined according towell-established techniques.

One embodiment is based on the recognition that if a Kalman filter isused, the probabilistic distribution is on the Gaussian form

({circumflex over (χ)}_(k|k) ^(i), P_(k|k) ^(ii), wherein {circumflexover (χ)}_(k|k) ^(i) is the first moment and P_(k|k) ^(ii) the secondmoment. If using a Kalman filter, updating the first two moments usingthe measurements y_(k) ^(i) is done with a Kalman gain K_(k) ^(i) For aset of N vehicles, one embodiment stacks the transformed state ofvehicles into the state vector {circumflex over (χ)}_(k|k)^(l)=[({circumflex over (χ)}_(k|k) ^(i))^(τ), . . . , ({circumflex over(χ)}_(k|k) ^(N))^(τ)]^(τ). One embodiment determines thecross-covariance between the state of vehicles as

${P_{k|k}^{ll} = {{{\mathbb{E}}\left\lbrack {\left( {x_{k} - {\hat{x}}_{k|k}^{l}} \right)\left( {x_{k} - {\hat{x}}_{k|k}^{l}} \right)^{\top}} \right\rbrack} = {{{\mathbb{E}}\left\lbrack {\begin{bmatrix}{x_{k}^{1} - {\hat{x}}_{k|k}^{1}} \\ \vdots \\{x_{k}^{N} - {\hat{x}}_{k|k}^{N}}\end{bmatrix}\left\lbrack \begin{matrix}{x_{k}^{1} - {\hat{x}}_{k|k}^{1}} \\ \vdots \\{x_{k}^{N} - {\hat{x}}_{k|k}^{N}}\end{matrix} \right\rbrack}^{\top} \right\rbrack} = \begin{bmatrix}P_{k|k}^{11} & \ldots & P_{k|k}^{1N} \\ \vdots & \ddots & \vdots \\P_{k|k}^{N1} & \ldots & P_{k|k}^{NN}\end{bmatrix}}}},$

Wherein P_(k|k) ^(ij)=(I−K_(k) ^(i)C^(ii))(A^(ii)P_(k−1|k−1)^(ij)(A^(jj))^(τ)+K_(k) ^(i)R^(ij)(K_(k) ^(j))^(τ)is thecross-covariance that is recursively determined as a function of theKalman gain, and wherein A^(ii) and C exemplifies a motion model andmeasurement model matrix, respectively.

Some embodiments employ stacked motion models, where the motion modelcan be a stacked vehicle dynamics model, a stacked constant accelerationmodel, a stacked constant position model, a stacked coordinated turnmodel, a stacked Singer model, or a combination of different stackedmodels, wherein the stacking means the stacking of multiple models aftereach other. For example, for a model of the motion of a vehicle

x_(k + 1)^(i) = A^(i)x_(k)^(i) + q_(k)^(x, i), ∀i = 1, …, ❘𝒜❘,n_(k + 1)^(i) = n_(k)^(i) + q_(k)^(n, i), ∀i = 1, …, ❘𝒜❘,θ_(k + 1)^(I) = θ_(k)^(I) + q_(k)^(I),θ_(k + 1)^(P) = θ_(k)^(P) + q_(k)^(P),θ_(k + 1)^(C) = θ_(k)^(C) + q_(k)^(C),where |

| is the number of vehicles,

x_(k + 1)^(i)n_(k + 1)^(i)are the motion states and ambiguity, and

$\begin{matrix}\theta_{k + 1}^{I} \\\theta_{k + 1}^{P} \\\theta_{k + 1}^{C}\end{matrix}$are residual bias states,

X_(k)^(g) = [(x_(k)¹)^(⊤), (x_(k)¹)^(⊤), …, (x_(k)^(|𝒜|))^(⊤), (x_(k)^(|𝒜|))^(⊤), (θ_(k)^(I))^(⊤), (θ_(k)^(P))^(⊤), (θ_(k)^(C))^(⊤)]^(⊤), q_(k)^(q) = [(q_(k)^(x, 1))^(⊤), (q_(k)^(n, 1))^(⊤), …, (q_(k)^(x, |𝒜❘))^(⊤), (q_(k)^(n, |𝒜❘))^(⊤), (q_(k)^(I))^(⊤), (q_(k)^(P))^(⊤), (q_(k)^(C))^(⊤)]^(⊤),is the stacked, augmented, state, and augmented process noise of themotion model. Similarly, the GNSS measurement model can be defined interms of the augmented state. For example, for a set of measurements

$\begin{matrix}{y_{k}^{P,i} =} & \left\lbrack {S^{i}\rho_{\mathcal{A}_{i,}k}^{S_{i}}} \right. & & {{+ S^{i}}M^{i}\theta_{k}^{I}} & {{+ S^{i}}M^{i}\theta_{k}^{P}} & & \left. {+ r_{k}^{P,i}} \right\rbrack \\{y_{k}^{\Phi,i} =} & \left\lbrack {S^{i}\rho_{\mathcal{A}_{i,}k}^{S_{i}}} \right. & {{+ \lambda}n_{k}^{i}} & {{- S^{i}}M^{i}\theta_{k}^{I}} & & {{+ S^{i}}M^{i}\theta_{k}^{C}} & \left. {+ r_{k}^{\Phi,i}} \right\rbrack \\{y_{k}^{I,i} =} & \lbrack & & {{+ M^{i}}\theta_{k}^{I}} & & & \left. {+ r_{k}^{I,i}} \right\rbrack \\{y_{k}^{{PRC},i} =} & \lbrack & & & {{+ M^{i}}\theta_{k}^{P}} & & \left. {+ r_{k}^{{PRC},i}} \right\rbrack \\{y_{k}^{{CPC},i} =} & \lbrack & & & & {{+ M^{i}}\theta_{k}^{C}} & {\left. {+ r_{k}^{{CPC},i}} \right\rbrack,}\end{matrix}$y_(k) ^(i)=[(y_(k) ^(P,i))^(τ)(y_(k) ^(Φ,i))^(τ)(y_(k)^(CPC,i))^(τ)]^(τ), the full measurement model includes the measurementsy_(k)=[(y_(k) ¹)^(τ), . . . , (y_(k) ^(N))^(τ)].

Another embodiment determines the cross-covariance between a state of avehicle and the augmented state of the remote server as a function ofthe joint Kalman gains for the state of the vehicles K_(k)^(l)=blkdiag(K_(k) ¹, . . . , K_(k) ^(N)) and the Kalman gain of theKalman filter of the remote server, as the combinationP _(k|k) ^(lc)=

[(χ_(k)−{circumflex over (χ)}_(k|k) ^(l))(χ_(k)−{circumflex over(χ)}_(k|k) ^(c))^(τ)]=(I−K _(k) ^(l) C ^(l))(A ^(l) P _(k−1|c−1) ^(lc) A^(τ) +Q)(I−K _(k) ^(c) C)^(τ) +K _(k) ^(l) R(K _(k))^(τ).

Alternatively or additionally, the Kalman gains are part of the receivedinformation 215 a from the vehicles.

Other embodiments are based on the recognition that when using aparticle filter, it can be problematic to compute the cross-covariancesrecursively because in such an approach the distributions are notrepresented only by the parameters as the first and second moment, butas a mixture of first and second moments as a Gaussian mixture. However,other embodiments recognize that for a particle filter the Kalman gainfor the most likely particle can instead be used.

Using the determined 620 e cross-covariance, the method 620 c fuses 630e the parameters of the probabilistic distribution of the state of thevehicles with the parameters of the probabilistic distribution of theaugmented state in the remote server. In one embodiment, the fusing ofthe first and second moments are done as a weighted combination of theinvolved first and second moment, respectively. For example, the fusedfirst moment is determined based on the weighted difference between thefirst moments, wherein the weight is a weighted combination of thecross-covariance Σ_(AB) and the individual covariance,χ^(f)=μ_(A)+(Σ_(A)−Σ_(AB))(Σ_(A)+Σ_(A)−Σ_(AB)−Σ_(AB)^(τ))⁻¹(μ_(B)−μ_(A)). Another embodiment determines the fused covariancebased on the weighted difference between the second moments, wherein theweight is a weighted combination of the cross-covariance Σ_(AB) and theindividual covariance,P^(f)=Σ_(A)−(Σ_(A)−Σ_(AB))(Σ_(A)+Σ_(A)−Σ_(AB)−Σ_(AB)^(τ))⁻¹(Σ_(A)−Σ_(AB) ^(τ)). Alternatively or additionally, the fusedfirst and second moment is determined asK ^(f)=((H ^(f))^(τ)(Σ^(p))⁻¹ H ^(f))⁻¹(H ^(f))^(τ)(Σ^(l))⁻¹,μ^(f) =K ^(f){circumflex over (χ)}^(p),Σ^(f) =K ^(f)Σ^(p)(K ^(f))^(τ)=((H ^(f))^(τ)(Σ^(p))⁻¹ H ^(f))⁻.

One embodiment recognizes that there are several inversions ofpotentially very high-dimensional matrices involved in fusing of theparameters. One embodiment uses Schur complements to reduce the size ofthe matrix that needs to be inverted to produce the fused covarianceΣ^(f).

Other embodiments are based on the understanding that when usingmeasurements from multiple vehicles, there is sometimes extrainformation that can be deduced from the measurements.

FIG. 6G shows an example of cross-covariance in the measurement noise ofthe full measurement model including measurements from multipletransmissions from multiple vehicles according to some embodiments. FIG.6G shows a matrix representing elements of the covariance of themeasurement noise. When stacking measurements resulting in an augmentedmeasurement model for the augmented state, but no satellites are sharedbetween vehicles, the diagonal 610 g will be the only part that hasnonzero components. However, when vehicles share the same satellite thatis used in creating the differenced satellite measurements, i.e., singledifference or double difference, the off-diagonals 620 g and 630 g can,if modeled properly, have nonzero components 640 g. Such nonzerocomponents are utilized by the probabilistic filter to increaseknowledge of the augmented state estimates.

Some embodiments understand that when the measurement noise matrix R hasbeen formed, no matter whether there are off-diagonal nonzero elementsor not, they can be used in the KF automatically. However, when havingnonzero off-diagonals, i.e., nonzero cross-covariance, the KF canutilize this to better determine the Kalman gain that determines theupdate of the first and second moment based on the measurement error.

In one embodiment, the measurement noise matrix is formed by singleand/or double differences of code and carrier phase measurements usingprinciples such that there is cross-covariance between measurements. Forexample, in some implementations, the non-zero off-diagonal elements 640are corresponding to a pair of vehicles and determined by a combinationof covariances of the corresponding pairs of vehicles.

FIG. 6H shows a flowchart of a method for determining the measurementnoise matrix such that it results in the structure in FIG. 6G. Themethod sorts 610 g of the satellite measurements, to form a structure ofsorted satellite measurements 615 h. For example, the measurements aresorted according to the elevation angle of the satellites, wherein thesatellite with the highest elevation angle is the first highest, thesecond highest is second until the satellite with the lowest elevationangle is sorted last.

Next, the method forms 620 h a difference between satellitemeasurements. This can be done in multiple ways, but the importance isto do it in such a manner that the correlation is visible. Oneembodiment defines a binary incidence matrix M^(i) for vehicle i,wherein the binary incidence matrix is matrix picking elements fromvectors. For example, let v ε

^(N) ^(S) be a residual bias vector consisting of all unique biases andv^(i)ε

^(N) ^(S) ^(ι) be the bias corresponding to the set of satellites S_(i)for vehicle i, wherein S={1, . . . , N_(S)} is the set of all of thepossibly visible satellites and S_(i)={s ε S| satellite s is observed byagent

_(i)} ⊆ S, |S_(i)|=N_(S) ^(i) is the set of satellites visible for avehicle i. Another embodiment defines a difference operator S^(i)=[1_(M)_(i) −I_(M) _(i) ▪]. Using the binary incidence matrix and thedifference operator, one embodiment forms the difference betweensatellite measurements as

ΔP_(𝒜_(i)B, k)^(S_(i)) = P_(ℬ, k)^(S_(i)) − P_(𝒜_(i), k)^(S_(i)), ΔΦ_(𝒜_(i)B, k)^(S_(i)) = Φ_(ℬ, k)^(S_(i)) − Φ_(𝒜_(i)k)^(S_(i)).,

wherein

P_(𝒜_(i), k)^(S_(i)) = M^(i)P_(𝒜_(i), k)^(S), Φ_(𝒜_(i), k)^(S_(i)) = M^(i)Φ_(𝒜_(i), k)^(S).is the code and carrier measurement for vehicle i, determined from theset of all code and carrier phase measurements

$\begin{matrix}P_{\mathcal{A}_{i},k}^{s} \\\Phi_{\mathcal{A}_{i},k}^{s}\end{matrix}.$

Other embodiments form the difference as the double-difference for avehicle i as

∇ΔP_(𝒜_(i), k) = S^(i)ΔP_(𝒜_(i)ℬ, k)^(S_(i)),∇ΔΦ_(𝒜_(i), k) = 𝒮^(i)ΔΦ_(𝒜_(i)ℬ, k)^(S_(i)).In one embodiment, the defined binary incidence matrix and differenceoperator are used to rewrite the difference as∇Δ

=S ^(i) Δ

=S ^(i)

=S ^(i)[M ^(i) P _(B,k) ^(S)−

]=S ^(i) M ^(i)[P _(B,k) ^(S)−

],∇Δ

=S ^(i) Δ

=S ^(i)

=S ^(i)[M ^(i)Φ_(B,k) ^(S) −

]=S ^(i)[M ^(i)Φ_(B,k) ^(S)−

].

Another embodiment models the nominal code and carrier noise as Gaussianrandom variables, with covariance Cov

=

and Cov

=

² when s_(l)=s_(m). In other words, the double-difference can beexpressed with the binary incidence matrix, difference operator, andnominal noise as

∇ΔP_(𝒜_(i), k) − 𝔼[∇ΔP_(𝒜_(i), k)] = S^(i)M^(i)(ϵ_(ℬ, k)^(S) − ϵ_(𝒜_(i), k)^(S))∇ΔΦ_(𝒜_(i), k) − 𝔼[∇ΔΦ_(𝒜_(i), k)] = S^(i)M^(i)(η_(ℬ, k)^(S) − η_(𝒜_(i), k)^(S)).Using the difference 625 h, the method determines 630 h the measurementnoise matrix having nonzero off-diagonal elements as illustrated in FIG.6G.

In one embodiment, determining 630 h the measurement noise matrix havingnonzero off-diagonal elements is done by choosing satellites with thesame base station when doing the differencing. In one embodiment thecovariance between a vehicle i and a vehicle j, is formed and this leadsto R_(ij,k) ^(ΦΦ)

Cov(∇Δ

, ∇Δ

)=S^(i)M^(i)R_(ηη,k)(M^(j))^(τ)(S^(j))^(τ), for the carrier phasemeasurements and similarly for the code measurements. In other words, bydetermining the differencing compared to the same base satellite, theupdate of the first and second moments of the augmented using the KF canbe done using extra information contained in the satellite measurements,because satellites are shared among vehicles, i.e., a satellite measureseveral vehicles.

FIG. 7 shows a server 700 for tracking states of multiple vehicles usingunsynchronized cooperation of information received from vehicles, e.g.,transmitted over an RF channel 709, according to some embodiments. Theserver includes an RF receiver 760 for receiving the first and secondmoment, and satellite measurements, and corresponding measurement noiseof said measurements from multiple transmissions from multiple vehicles.

The system includes a memory 780 that stores 781 a probabilisticmeasurement model relating incoming satellite measurements with a stateof a vehicle. The memory also stores 782 a probabilistic motion modelrelating a previous state of a vehicle with a prediction of a state of avehicle according to the motion model. For example, the motion model canbe a stacked vehicle dynamics model, a stacked constant accelerationmodel, a stacked constant position model, a stacked coordinated turnmodel, a stacked Singer model, or a combination of different stackedmodels, wherein the stacking means the stacking of multiple models aftereach other.

The memory also stores instructions 783 to execute a probabilisticfilter according to some embodiments of the invention.

The probabilistic system 700 includes an RF measurement module 740 thatis operatively connected 750 to an RF receiver 760 that receivesinformation 709 transmitted over an RF channel from multiple vehicles.In some embodiments, the information includes a first moment and asecond moment of multiple vehicles. In other embodiments, theinformation includes satellite signals received from said vehicles,wherein the signals include satellite measurements, positioninformation, and timing information.

Some embodiments relate the first and second moments of the state of aprevious time step with the first and second moments of the state in acurrent time step using the motion model. For example, one embodimentpropagates the first and second moments of the state of the vehicle intime till a current instance of time using a model of time propagationusing the motion model. Examples of the model of time propagationinclude a Singer model, a constant acceleration model, and asingle-track vehicle model. Using both moments in time propagation isadvantageous over propagating just a first moment because thepropagation of uncertainty, hence how much the first moment should betrusted when updating the state, is taken into account. In otherembodiments, the time propagation is done by a combination of a motionmodel of the vehicle and the measurements of the vehicle.

In various embodiments, the probabilistic system uses the first andsecond moment and satellite measurements to update the first and secondmoments using information received from multiple transmissions frommultiple vehicles. For example, the GNSS signals are received and uponreceiving the signals, a processor 730 determines 733 updated first andsecond moments using a probabilistic filter 732.

In other embodiments, the probabilistic system 700 includes an RFtransmitter 770 for transmitting 739 the updated first and secondmoment, i.e., mean and covariance, that corresponds to a vehicle. Forexample, the mean and covariance of vehicle 1 are transmitted to vehicle1.

Exemplar Embodiments

FIG. 8 shows an example of a vehicle-to-vehicle (V2V) communication andplanning based on state estimation according to one embodiment. As usedherein, each vehicle can be any type of moving transportation system,including a passenger car, a mobile robot, or a rover. For example, avehicle can be autonomous or semi-autonomous.

In this example, multiple vehicles 800, 810, 820, are moving on a givenfreeway 801. Each vehicle can make many motions. For example, thevehicles can stay on the same path 850, 890, 880, or can change paths(or lanes) 860, 870. Each vehicle has its own sensing capabilities,e.g., Lidars, cameras, etc. Each vehicle has the possibility to transmitand receive 830, 840 information with its neighboring vehicles and/orcan exchange information indirectly through other vehicles via a remoteserver. For example, vehicles 800 and 880 can exchange informationthrough vehicle 810. With this type of communication network, theinformation can be transmitted over a large portion of the freeway orhighway 801.

Some embodiments are configured to address the following scenario. Forexample, vehicle 820 wants to change its path and chooses option 870 inits path planning. However, at the same time, vehicle 810 also choosesto change its lane and wants to follow option 860. In this case, the twovehicles might collide, or at best vehicle 810 will have to execute anemergency brake to avoid colliding with vehicle 820. This is where thepresent invention can help. To that end, some embodiments enable thevehicles to transmit not only what the vehicles sense at the presenttime instant t, but also, additionally or alternatively, transmit whatthe vehicles are planning to do at time T+δ_(t).

In the example of FIG. 8 , vehicle 820 informs of its plan to changelane to vehicle 810 after planning and committing to execute its plan.Thus, vehicle 810 knows that in δ_(t) time interval the vehicle 820 isplanning to make a move to its left 870. Accordingly, vehicles 810 canselect the motion 890 instead of 860, i.e., staying on the same lane.

Additionally or alternatively, the motion of the vehicles can be jointlycontrolled by the remote server based on state estimations determinedcooperatively. For example, in some embodiments, the multiple vehiclesdetermined for joint state estimation are the vehicles that form andpotentially can form a platoon of vehicles jointly controlled with ashared control objective.

FIG. 9 is a schematic of a multi-vehicle platoon shaping for an accidentavoidance scenario according to one embodiment. For example, consider agroup of vehicles 930, 970, 950, 960, moving on a freeway 901. Considernow that suddenly, there is an accident ahead of the vehicle platoon inzone 900. This accident renders zone 900 unsafe for the vehicles tomove. The vehicles 920, 960 sense the problem for example with a camera,and communicate this information to the vehicles 930, 970. The platoonthen executes a distributed optimization algorithm, e.g., formationkeeping multi-agent algorithm, which selects the best shape of theplatoon to avoid the accident zone 900 and also to keep the vehicle flowuninterrupted. In this illustrative example, the best shape of theplatoon is to align and form a line 995, avoiding zone 900.

FIG. 10 shows a block diagram of a system 1000 for direct and indirectcontrol of mixed-autonomy vehicles in accordance with some embodiments.The system 1000 can be arranged on a remote server as part of RSU tocontrol the passing mixed-autonomy vehicles including autonomous,semiautonomous, and/or manually driven vehicles. The system 1000 canhave a number of interfaces connecting the system 1000 with othermachines and devices. A network interface controller (NIC) 1050 includesa receiver adapted to connect the system 1000 through the bus 1006 to anetwork 1090 connecting the system 1000 with the mixed-automata vehiclesto receive a traffic state of a group of mixed-autonomy vehiclestraveling in the same direction, wherein the group of mixed-autonomyvehicles includes controlled vehicles willing to participate in aplatoon formation and at least one uncontrolled vehicle, and wherein thetraffic state is indicative of a state of each vehicle in the group andthe controlled vehicle. For example, in one embodiment the traffic stateincludes current headways, current speeds, and current acceleration ofthe mixed-automata vehicles. In some embodiments, the mixed-automatavehicles include all uncontrolled vehicles within a predetermined rangefrom flanking controlled vehicles in the platoon.

The NIC 1050 also includes a transmitter adapted to transmit the controlcommands to the controlled vehicles via the network 1090. To that end,system 1000 includes an output interface, e.g., a control interface1070, configured to submit the control commands 1075 to the controlledvehicles in the group of mixed-autonomy vehicles through the network1090. In such a manner, the system 1000 can be arranged on a remoteserver in direct or indirect wireless communication with themixed-automata vehicles.

The system 1000 can also include other types of input and outputinterfaces. For example, system 1000 can include a human-machineinterface 1010. The human-machine interface 1010 can connect thecontroller 1000 to a keyboard 1011 and pointing device 1012, wherein thepointing device 1012 can include a mouse, trackball, touchpad, joystick,pointing stick, stylus, or touchscreen, among others.

The system 1000 includes a processor 1020 configured to execute storedinstructions, as well as a memory 1040 that stores instructions that areexecutable by the processor. The processor 1020 can be a single-coreprocessor, a multi-core processor, a computing cluster, or any number ofother configurations. The memory 1040 can include random access memory(RAM), read-only memory (ROM), flash memory, or any other suitablememory machines. The processor 1020 can be connected through the bus1006 to one or more input and output devices.

The processor 1020 is operatively connected to a memory storage 1030storing the instruction as well as processing data used by theinstructions. The storage 1030 can form a part of or be operativelyconnected to the memory 1040. For example, the memory can be configuredto store a probabilistic filter 1031 trained to track the augmentedstate of mixed-automata vehicles, transform the traffic state intotarget headways for the mixed-autonomy vehicles; and store a one ormultiple models 1033 configured to explain the motion of the vehicles.For example, models 1033 can include motion models, measurement models,traffic models, and the like.

The processor 1020 is configured to determine control commands for thecontrolled vehicles that indirectly control the uncontrolled vehicles aswell. To that end, the processor is configured to execute a controlgenerator 1032 to determine control commands based on the state of thevehicles. In some embodiments, the control generator 1032 uses a deepreinforcement learning (DRL) controller trained to generate controlcommands from the augmented state for an individual and/or a platoon ofvehicles.

FIG. 11A shows a schematic of a vehicle 1101 controlled directly orindirectly according to some embodiments. As used herein, vehicle 1101can be any type of wheeled vehicle, such as a passenger car, bus, orrover. Also, vehicle 1101 can be autonomous or semi-autonomous. Forexample, some embodiments control the motion of vehicle 1101. Examplesof the motion include the lateral motion of the vehicle controlled by asteering system 1103 of the vehicle 1101. In one embodiment, thesteering system 1103 is controlled by the controller 1102 incommunication with the system 1000. Additionally or alternatively, thesteering system 1103 can be controlled by a driver of vehicle 1101.

The vehicle can also include an engine 1106, which can be controlled bythe controller 1102 or by other components of the vehicle 1101. Thevehicle can also include one or more sensors 1104 to sense thesurrounding environment. Examples of sensors 1104 include distance rangefinders, radars, lidars, and cameras. The vehicle 1101 can also includeone or more sensors 1105 to sense its current motion quantities andinternal status. Examples of the sensors 1105 include global positioningsystem (GPS), accelerometers, inertial measurement units, gyroscopes,shaft rotational sensors, torque sensors, deflection sensors, pressuresensor, and flow sensors. The sensors provide information to thecontroller 1102. The vehicle can be equipped with a transceiver 1106enabling communication capabilities of the controller 1102 through wiredor wireless communication channels.

FIG. 11B shows a schematic of interaction between controller 1102receiving controlled commands from the system 1000 and the controller1100 of the vehicle 1101 according to some embodiments. For example, insome embodiments, the controllers 1100 of the vehicle 1101 are steering1110 and brake/throttle controllers 1120 that controls rotation andacceleration of the vehicle 1100. In such a case, controller 1102outputs control inputs to the controllers 1110 and 1120 to control thestate of the vehicle. The controllers 1100 can also include high-levelcontrollers, e.g., a lane-keeping assist controller 1130 that furtherprocess the control inputs of the predictive controller 1102. In bothcases, the controllers 1100 maps use the outputs of the predictivecontroller 1102 to control at least one actuator of the vehicle, such asthe steering wheel and/or the brakes of the vehicle, in order to controlthe motion of the vehicle. States x_(t) of the vehicular machine couldinclude position, orientation, and longitudinal/lateral velocities;control inputs u_(t) could include lateral/longitudinal acceleration,steering angles, and engine/brake torques. State constraints on thissystem can include lane-keeping constraints and obstacle-avoidanceconstraints. Control input constraints may include steering angleconstraints and acceleration constraints. Collected data could includeposition, orientation, and velocity profiles, accelerations, torques,and/or steering angles.

The above-described embodiments of the present invention can beimplemented in numerous ways. For example, the embodiments may beimplemented using hardware, software, or a combination thereof. Whenimplemented in software, the software code can be executed on anysuitable processor or collection of processors, whether provided in asingle computer or distributed among multiple computers. Such processorsmay be implemented as integrated circuits, with one or more processorsin an integrated circuit component. Though, a processor may beimplemented using circuitry in any suitable format.

Also, the various methods or processes outlined herein may be coded assoftware that is executable on one or more processors that employ anyone of a variety of operating systems or platforms. Additionally, suchsoftware may be written using any of a number of suitable programminglanguages and/or programming or scripting tools, and also may becompiled as executable machine language code or intermediate code thatis executed on a framework or virtual machine. Typically, thefunctionality of the program modules may be combined or distributed asdesired in various embodiments.

Also, the embodiments of the invention may be embodied as a method, ofwhich an example has been provided. The acts performed as part of themethod may be ordered in any suitable way. Accordingly, embodiments maybe constructed in which acts are performed in an order different thanillustrated, which may include performing some acts concurrently, eventhough shown as sequential acts in illustrative embodiments.

Although the invention has been described by way of examples ofpreferred embodiments, it is to be understood that various otheradaptations and modifications can be made within the spirit and scope ofthe invention. Therefore, it is the object of the appended claims tocover all such variations and modifications as come within the truespirit and scope of the invention.

The invention claimed is:
 1. A probabilistic system for tracking a stateof a vehicle using unsynchronized cooperation of information receivedfrom satellite signals transmitted by a global navigation satellitesystem (GNSS) and information transmitted over radio frequency (RF)channel, comprising: a memory configured to store a probabilisticmulti-head measurement model relating incoming measurements with thestate of the vehicle, wherein the probabilistic multi-head measurementmodel includes a first head relating measurements of the satellitesignals subject to measurement noise with a belief on the state of thevehicle, and a second head relating an estimation of the state of thevehicle subject to estimation noise with the belief on the state of thevehicle; and at least one processor configured to process executableinstructions of modules of the probabilistic system, the modulescomprising: a GNSS measurement module operatively connected to a GNSSreceiver and configured to determine the measurements of the satellitesignals and the measurement noise; an RF measurement module operativelyconnected to an RF receiver of the vehicle and configured to receivedata indicative of the estimation of the state of the vehicle and theestimation noise, wherein the received data for a previous instance oftime indicate the estimation of the state of the vehicle and theestimation noise for the previous instance of time; and a probabilisticfilter configured to update recursively parameters of a probabilisticdistribution of the state of the vehicle to produce the belief on thestate of the vehicle based on the multi-head measurement model acceptingone or a combination of the measurements of the satellite signalssubject to the measurement noise and the estimation of the state of thevehicle subject to the estimation noise, wherein to update theparameters of the probabilistic distribution of the state of the vehicleat a current instance of time, the estimation of the state of thevehicle and the estimation noise for the previous instance of time arepropagated till the current instance of time using a model of timepropagation.
 2. The probabilistic system of claim 1, wherein theprobabilistic filter propagates recursively the parameters of theprobabilistic distribution of the state of the vehicle according to amotion model of state transitions of the vehicle subject to processnoise and updates the parameters of the probabilistic distribution uponreceiving outputs of one or a combination of the first head ofmulti-head measurement model and the second head of multi-headmeasurement model.
 3. The probabilistic system of claim 1, wherein thestate of the vehicle includes an ambiguity of propagation of thesatellite signals, and one or multiple state biases capturing residualerrors in atmospheric delays.
 4. The probabilistic system of claim 1,wherein upon receiving the measurements of the satellite signals and themeasurement noise, the probabilistic filter executes the first head ofthe multi-head measurement model to estimate a measurement of the stateof the vehicle conforming to the measurements of the satellite signalsto produce a measured state of the vehicle; transform the measurementnoise to a second moment representing a covariance of the measured stateof the vehicle; and compare the belief on the state of the vehicle withthe measured state of the vehicle measured with the covariance to outputresults of the comparison.
 5. The probabilistic system of claim 4,wherein the output of the first head of the multi-head measurement modelincludes an error between the measured state of the vehicle and thecurrent belief of the state of the vehicle inserted into the measurementmodel subject to covariance of the measurement noise.
 6. Theprobabilistic system of claim 4, wherein the output of the first head ofthe multi-head measurement model includes the measured state of thevehicle, the current belief of the state provided to the first head ofthe multi-head measurement model inserted into the measurement model,and covariance of the measurement noise.
 7. The probabilistic system ofclaim 4, wherein upon receiving the estimation of the state of thevehicle and the estimation noise, the probabilistic filter executes thesecond head of the multi-head measurement model to transform thereceived estimation of the state of the vehicle into an estimated stateof the vehicle; transform the received estimation noise into acovariance of the estimated state of the vehicle conforming to thecovariance of the measured state of the vehicle; and compare the currentbelief on the state of the vehicle with the estimated state of thevehicle estimated with the covariance of the estimated state of thevehicle to output results of the comparison.
 8. The probabilistic systemof claim 7, wherein the received estimation of the state of the vehicleis a first moment of the state of the vehicle, and wherein the receivedestimation noise is a second moment of the state of the vehicle.
 9. Theprobabilistic system of claim 8, wherein the covariance of the estimatedstate of the vehicle is determined as a weighted combination of thesecond moment of the estimation of the state of the vehicle and a secondmoment of the probabilistic distribution of the belief of the state ofthe vehicle.
 10. The probabilistic system of claim 8, wherein thecovariance of the estimated state of the vehicle is determined based ona difference between a probabilistic distribution defined by thereceived first and second moments of the estimation of the state of thevehicle and the probabilistic distribution of the belief of the state ofthe vehicle defined by the parameters maintained by the probabilisticfilter.
 11. The probabilistic system of claim 8, wherein the estimatedstate of the vehicle is determined as a weighted combination of thebelief of the state of the vehicle and the received first moment of thestate of the vehicle, wherein the weighting includes the covariance ofthe estimation noise.
 12. The probabilistic system of claim 2, whereinthe probabilistic filter propagates the parameters of the probabilisticdistribution of the state of the vehicle at each instance of time, andupdate, for the current instance of time, the parameters of theprobabilistic distribution of the state of the vehicle based on theoutput of the first head or the output of the second head of themulti-head measurement model.
 13. The probabilistic system of claim 2,wherein the probabilistic filter propagates the parameters of theprobabilistic distribution of the state of the vehicle at each instanceof time, and update, for the current instance of time, the parameters ofthe probabilistic distribution of the state of the vehicle based on acombination of the output of the first head and the output of the secondhead of the multi-head measurement model merged into an augmented outputof the multi-head measurement model.
 14. The probabilistic system ofclaim 1, wherein the model of time propagation includes one or acombination of a motion model of the vehicle, a Singer model, a constantacceleration model, and a single-track vehicle model.
 15. Theprobabilistic system of claim 1, wherein the probabilistic filter is oneor a combination of a mixed-integer extended Kalman filter, amixed-integer linear-regression Kalman filter, a linear Kalman filter,an extended Kalman filter, a particle filter.
 16. The probabilisticsystem of claim 1, wherein the RF measurement module receives theestimation of the state of the vehicle and the estimation noisetransmitted by a road-side unit (RSU).
 17. The probabilistic system ofclaim 1, further comprising: a transmitter configured to transmit to aremote server the parameters of the probabilistic distribution of thestate of the vehicle, the parameters including at least a first and asecond moment of the probabilistic distribution, and the measurements ofsatellite signals, such that the RF measurement module receives theestimation of the state of the vehicle and the estimation noise inresponse to the transmission.
 18. The probabilistic system of claim 17,wherein the remote server is configured to receive multipletransmissions from multiple vehicles including the transmission from thevehicle, each transmission from a corresponding vehicle includes themeasurements of satellite signals and the parameters of a probabilisticdistribution of the state of the corresponding vehicle; fuse the statesof the multiple vehicles defined by the parameters of the correspondingprobabilistic distributions into an augmented state; fuse themeasurements of satellite signals of the multiple vehicles into anaugmented measurement of the augmented state subject to augmentedmeasurement noise defined by a non-diagonal covariance matrix; execute aprobabilistic filter updating the augmented state based on the augmentedmeasurement subject to the augmented measurement noise; fuse theparameters of the probabilistic distribution of the state of the vehiclewith a portion of the updated augmented state corresponding to thevehicle; and transmit the fused parameters of the probabilisticdistribution of the state of the vehicle to the RF module of thevehicle.
 19. A probabilistic method for tracking a state of a vehicleusing unsynchronized cooperation of information received from satellitesignals transmitted by a global navigation satellite system (GNSS) andinformation transmitted over radio frequency (RF) channel, wherein themethod uses a processor coupled to a memory storing a probabilisticmulti-head measurement model relating incoming measurements with thestate of the vehicle, wherein the probabilistic multi-head measurementmodel includes a first head relating measurements of the satellitesignals subject to measurement noise with a belief on the state of thevehicle, and a second head relating an estimation of the state of thevehicle subject to estimation noise with the belief on the state of thevehicle, wherein the processor is coupled with stored instructionsimplementing the method, wherein the instructions, when executed by theprocessor carry out steps of the method, comprising: determining, usinga GNSS receiver, the measurements of the satellite signals and themeasurement noise; receiving from an RF receiver of the vehicle, dataindicative of the estimation of the state of the vehicle and theestimation noise, wherein the received data for a previous instance oftime indicate the estimation of the state of the vehicle and theestimation noise for the previous instance of time; and updatingrecursively parameters of a probabilistic distribution of the state ofthe vehicle to produce the belief on the state of the vehicle based onthe multi-head measurement model accepting one or a combination of themeasurements of the satellite signals subject to the measurement noiseand the estimation of the state of the vehicle subject to the estimationnoise, wherein to update the parameters of the probabilisticdistribution of the state of the vehicle at a current instance of time,the estimation of the state of the vehicle and the estimation noise forthe previous instance of time are propagated till the current instanceof time using a model of time propagation.